[
  {
    "path": ".gitignore",
    "content": "*.kdev4\n"
  },
  {
    "path": "README.md",
    "content": "# InGVIO\n\nAn invariant filter for visual-inertial-raw GNSS fusion.\n\n**Paper:** InGVIO: A Consistent Invariant Filter For Fast and High-Accuracy GNSS-Visual-Inertial Odometry\n\n**Paper Author:** Changwu Liu, Chen Jiang and Haowen Wang\n\n**Paper Status:** Manuscript submitted to RA-L for possible publication. Preprint version available on ArXiv.\n\n**Current Paper Link:** https://arxiv.org/abs/2210.15145\n\nInGVIO 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.\n\nMoreover, we offer our fixed-wing datasets in the form of ROS Bags including stereo-visual, IMU and raw-GNSS measurements.\n\n**Fixed-Wing Dataset Link:** https://cloud.tsinghua.edu.cn/d/65ecf6768563421faaed/\n                             password to extract: LCW@uav_18\n                            \n**Fixed-Wing Dataset Videos:** The videos are uploaded to the link that stores the fixed-wing datasets.\n\nThe 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'.\n\n## 1. System Requirements\n\n### 1.1  Support of C++ 14 Features\n\nThe compiler should at least support c++14 standards.\n\n### 1.2  ROS-Noetic System\n\nInGVIO 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.\n\n### 1.3  Eigen Library\n\nEigen 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.\n\n### 1.4  SuiteSparse Library\n\nWe use [SuiteSparse](https://github.com/DrTimothyAldenDavis/SuiteSparse/releases) Library for sparse QR-decomposition in visual updates. \n\n### 1.5  gnss_comm Library\n\nA 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.\n\n## 2. Build InGVIO\n\nDownload or clone this repo to your ROS workspace.\n\n```\ncd ~/ws_catkin\ncatkin_make\n```\n\nSource the setup file to let ROS recognize the related launch files.\n\n```\nsource devel/setup.bash\n```\n\n## 3. Run with Our Fixed-Wing Datasets\n\nFirst download and unzip our fixed-wing dataset to your environment.\n\nThe 'fw_zed2i_f9p' folder contains the configuration files for fixed-wing datasets. To run InGVIO in your environment, the following should be conducted first.\n\nPlease 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.\n\nIf you want to record the output topics, please set your output dir in 'ingvio/launch/ingvio_mono, stereo_fw_record.launch' files.\n\nTo run with monocular camera, please enter:\n\n```\nroslaunch ingvio_estimator ingvio_mono_fw.launch\n```\n\nFor stereo-case, please enter:\n\n```\nroslaunch ingvio_estimator ingvio_stereo_fw.launch\n```\n\nTo record the output topics, you can directly use our script file by:\n\n```\nroslaunch ingvio_estimator ingvio_mono_fw_record.launch\nroslaunch ingvio_estimator ingvio_stereo_fw_record.launch\n```\n\nRviz will be automatically launched. Run the ROS Bag to see the results:\n\n```\nrosbag play fw_easy.bag --pause\n```\n\n## 4. Run with GVINS Public Datasets\n\n### 4.1 Configuration Settings\n\nThe 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.\n\nTo run InGVIO in your environment, the following should be conducted first.\n\nPlease 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.\n\nIf you want to record the output topics, please set your output dir in 'ingvio/launch/ingvio_mono, stereo_sf_record.launch' files.\n\nTo run with monocular camera, please enter:\n\n```\nroslaunch ingvio_estimator ingvio_mono_sf.launch\n```\n\nFor stereo-case, please enter:\n\n```\nroslaunch ingvio_estimator ingvio_stereo_sf.launch\n```\n\nTo record the output topics, you can directly use our script file by:\n\n```\nroslaunch ingvio_estimator ingvio_mono_sf_record.launch\nroslaunch ingvio_estimator ingvio_stereo_sf_record.launch\n```\n\nRviz will be automatically launched. Run the ROS Bag to see the results:\n\n```\nrosbag play sports_field.bag --pause\nrosbag play complex_environment.bag --pause\nrosbag play urban_driving.bag --pause\n```\n\n### 4.2 Parameter Tuning Hints\n\n**For sports_field.bag:** Monocular/Stereo Case: max_pts_frame = 150/110, visual_noise = 0.12, gnss_chi2_test = false, imu_buffer_size = 3000.\n\n**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.\n\n**For indoors_outdoors.bag:** Monocular/Stereo Case: max_pts_frame = 150/110, visual_noise = 0.18, gnss_chi2_test = false, gnss_strong_reject = true. \n\nPlease modify or try other parameters if the above behave not well. Good parameters may be different under different environment settings.\n\n## 5. Acknowledgements\n\nThe 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.\n\nThe [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.\n\n## 6. License\n\nThis software is open-sourced under GPLv3 license. See [GNU GPL v3.0 - GNU](https://www.gnu.org/licenses/gpl-3.0.html).\n\nPlease cite our paper if you use either our code or our fixed-wing datasets.\n"
  },
  {
    "path": "camera_model/CMakeLists.txt",
    "content": "cmake_minimum_required(VERSION 2.8.3)\nproject(camera_model)\n\nset(CMAKE_BUILD_TYPE \"Release\")\nset(CMAKE_CXX_FLAGS \"${CMAKE_CXX_FLAGS} -std=c++11\")\n\nfind_package(catkin REQUIRED COMPONENTS\n    roscpp\n    std_msgs\n)\n\nif(EXISTS \"/usr/include/eigen3\")\n    include_directories(\"/usr/include/eigen3\")\nelse()\n    find_package(Eigen3 REQUIRED)\n    include_directories(${EIGEN3_INCLUDE_DIR})\nendif()\n\nfind_package(Boost REQUIRED COMPONENTS filesystem program_options system)\nfind_package(OpenCV REQUIRED)\n\ncatkin_package(\n    INCLUDE_DIRS include\n    LIBRARIES camera_model\n    CATKIN_DEPENDS roscpp std_msgs\n)\n\ninclude_directories(\n    ${PROJECT_SOURCE_DIR}/include\n    ${catkin_INCLUDE_DIRS}\n    ${OpenCV_INCLUDE_DIRS}\n    ${Boost_INCLUDE_DIRS}\n)\n\n# if(EXISTS \"${PROJECT_SOURCE_DIR}/test/\")\n#     enable_testing()\n#     add_subdirectory(${PROJECT_SOURCE_DIR}/test)\n# endif()\n\nadd_library(camera_model\n    src/camera.cpp\n    src/MeiCamera.cpp\n    src/PinholeCamera.cpp\n    src/camera_factory.cpp\n)\n\ntarget_link_libraries(camera_model ${Boost_LIBRARIES} ${OpenCV_LIBS})\n"
  },
  {
    "path": "camera_model/LICENSE",
    "content": "                    GNU GENERAL PUBLIC LICENSE\n                       Version 3, 29 June 2007\n\n Copyright (C) 2007 Free Software Foundation, Inc. <https://fsf.org/>\n Everyone is permitted to copy and distribute verbatim copies\n of this license document, but changing it is not allowed.\n\n                            Preamble\n\n  The GNU General Public License is a free, copyleft license for\nsoftware and other kinds of works.\n\n  The licenses for most software and other practical works are designed\nto take away your freedom to share and change the works.  By contrast,\nthe GNU General Public License is intended to guarantee your freedom to\nshare and change all versions of a program--to make sure it remains free\nsoftware for all its users.  We, the Free Software Foundation, use the\nGNU General Public License for most of our software; it applies also to\nany other work released this way by its authors.  You can apply it to\nyour programs, too.\n\n  When we speak of free software, we are referring to freedom, not\nprice.  Our General Public Licenses are designed to make sure that you\nhave the freedom to distribute copies of free software (and charge for\nthem if you wish), that you receive source code or can get it if you\nwant it, that you can change the software or use pieces of it in new\nfree programs, and that you know you can do these things.\n\n  To protect your rights, we need to prevent others from denying you\nthese rights or asking you to surrender the rights.  Therefore, you have\ncertain responsibilities if you distribute copies of the software, or if\nyou modify it: responsibilities to respect the freedom of others.\n\n  For example, if you distribute copies of such a program, whether\ngratis or for a fee, you must pass on to the recipients the same\nfreedoms that you received.  You must make sure that they, too, receive\nor can get the source code.  And you must show them these terms so they\nknow their rights.\n\n  Developers that use the GNU GPL protect your rights with two steps:\n(1) assert copyright on the software, and (2) offer you this License\ngiving you legal permission to copy, distribute and/or modify it.\n\n  For the developers' and authors' protection, the GPL clearly explains\nthat there is no warranty for this free software.  For both users' and\nauthors' sake, the GPL requires that modified versions be marked as\nchanged, so that their problems will not be attributed erroneously to\nauthors of previous versions.\n\n  Some devices are designed to deny users access to install or run\nmodified versions of the software inside them, although the manufacturer\ncan do so.  This is fundamentally incompatible with the aim of\nprotecting users' freedom to change the software.  The systematic\npattern of such abuse occurs in the area of products for individuals to\nuse, which is precisely where it is most unacceptable.  Therefore, we\nhave designed this version of the GPL to prohibit the practice for those\nproducts.  If such problems arise substantially in other domains, we\nstand ready to extend this provision to those domains in future versions\nof the GPL, as needed to protect the freedom of users.\n\n  Finally, every program is threatened constantly by software patents.\nStates should not allow patents to restrict development and use of\nsoftware on general-purpose computers, but in those that do, we wish to\navoid the special danger that patents applied to a free program could\nmake it effectively proprietary.  To prevent this, the GPL assures that\npatents cannot be used to render the program non-free.\n\n  The precise terms and conditions for copying, distribution and\nmodification follow.\n\n                       TERMS AND CONDITIONS\n\n  0. Definitions.\n\n  \"This License\" refers to version 3 of the GNU General Public License.\n\n  \"Copyright\" also means copyright-like laws that apply to other kinds of\nworks, such as semiconductor masks.\n\n  \"The Program\" refers to any copyrightable work licensed under this\nLicense.  Each licensee is addressed as \"you\".  \"Licensees\" and\n\"recipients\" may be individuals or organizations.\n\n  To \"modify\" a work means to copy from or adapt all or part of the work\nin a fashion requiring copyright permission, other than the making of an\nexact copy.  The resulting work is called a \"modified version\" of the\nearlier work or a work \"based on\" the earlier work.\n\n  A \"covered work\" means either the unmodified Program or a work based\non the Program.\n\n  To \"propagate\" a work means to do anything with it that, without\npermission, would make you directly or secondarily liable for\ninfringement under applicable copyright law, except executing it on a\ncomputer or modifying a private copy.  Propagation includes copying,\ndistribution (with or without modification), making available to the\npublic, and in some countries other activities as well.\n\n  To \"convey\" a work means any kind of propagation that enables other\nparties to make or receive copies.  Mere interaction with a user through\na computer network, with no transfer of a copy, is not conveying.\n\n  An interactive user interface displays \"Appropriate Legal Notices\"\nto the extent that it includes a convenient and prominently visible\nfeature that (1) displays an appropriate copyright notice, and (2)\ntells the user that there is no warranty for the work (except to the\nextent that warranties are provided), that licensees may convey the\nwork under this License, and how to view a copy of this License.  If\nthe interface presents a list of user commands or options, such as a\nmenu, a prominent item in the list meets this criterion.\n\n  1. Source Code.\n\n  The \"source code\" for a work means the preferred form of the work\nfor making modifications to it.  \"Object code\" means any non-source\nform of a work.\n\n  A \"Standard Interface\" means an interface that either is an official\nstandard defined by a recognized standards body, or, in the case of\ninterfaces specified for a particular programming language, one that\nis widely used among developers working in that language.\n\n  The \"System Libraries\" of an executable work include anything, other\nthan the work as a whole, that (a) is included in the normal form of\npackaging a Major Component, but which is not part of that Major\nComponent, and (b) serves only to enable use of the work with that\nMajor Component, or to implement a Standard Interface for which an\nimplementation is available to the public in source code form.  A\n\"Major Component\", in this context, means a major essential component\n(kernel, window system, and so on) of the specific operating system\n(if any) on which the executable work runs, or a compiler used to\nproduce the work, or an object code interpreter used to run it.\n\n  The \"Corresponding Source\" for a work in object code form means all\nthe source code needed to generate, install, and (for an executable\nwork) run the object code and to modify the work, including scripts to\ncontrol those activities.  However, it does not include the work's\nSystem Libraries, or general-purpose tools or generally available free\nprograms which are used unmodified in performing those activities but\nwhich are not part of the work.  For example, Corresponding Source\nincludes interface definition files associated with source files for\nthe work, and the source code for shared libraries and dynamically\nlinked subprograms that the work is specifically designed to require,\nsuch as by intimate data communication or control flow between those\nsubprograms and other parts of the work.\n\n  The Corresponding Source need not include anything that users\ncan regenerate automatically from other parts of the Corresponding\nSource.\n\n  The Corresponding Source for a work in source code form is that\nsame work.\n\n  2. Basic Permissions.\n\n  All rights granted under this License are granted for the term of\ncopyright on the Program, and are irrevocable provided the stated\nconditions are met.  This License explicitly affirms your unlimited\npermission to run the unmodified Program.  The output from running a\ncovered work is covered by this License only if the output, given its\ncontent, constitutes a covered work.  This License acknowledges your\nrights of fair use or other equivalent, as provided by copyright law.\n\n  You may make, run and propagate covered works that you do not\nconvey, without conditions so long as your license otherwise remains\nin force.  You may convey covered works to others for the sole purpose\nof having them make modifications exclusively for you, or provide you\nwith facilities for running those works, provided that you comply with\nthe terms of this License in conveying all material for which you do\nnot control copyright.  Those thus making or running the covered works\nfor you must do so exclusively on your behalf, under your direction\nand control, on terms that prohibit them from making any copies of\nyour copyrighted material outside their relationship with you.\n\n  Conveying under any other circumstances is permitted solely under\nthe conditions stated below.  Sublicensing is not allowed; section 10\nmakes it unnecessary.\n\n  3. Protecting Users' Legal Rights From Anti-Circumvention Law.\n\n  No covered work shall be deemed part of an effective technological\nmeasure under any applicable law fulfilling obligations under article\n11 of the WIPO copyright treaty adopted on 20 December 1996, or\nsimilar laws prohibiting or restricting circumvention of such\nmeasures.\n\n  When you convey a covered work, you waive any legal power to forbid\ncircumvention of technological measures to the extent such circumvention\nis effected by exercising rights under this License with respect to\nthe covered work, and you disclaim any intention to limit operation or\nmodification of the work as a means of enforcing, against the work's\nusers, your or third parties' legal rights to forbid circumvention of\ntechnological measures.\n\n  4. Conveying Verbatim Copies.\n\n  You may convey verbatim copies of the Program's source code as you\nreceive it, in any medium, provided that you conspicuously and\nappropriately publish on each copy an appropriate copyright notice;\nkeep intact all notices stating that this License and any\nnon-permissive terms added in accord with section 7 apply to the code;\nkeep intact all notices of the absence of any warranty; and give all\nrecipients a copy of this License along with the Program.\n\n  You may charge any price or no price for each copy that you convey,\nand you may offer support or warranty protection for a fee.\n\n  5. Conveying Modified Source Versions.\n\n  You may convey a work based on the Program, or the modifications to\nproduce it from the Program, in the form of source code under the\nterms of section 4, provided that you also meet all of these conditions:\n\n    a) The work must carry prominent notices stating that you modified\n    it, and giving a relevant date.\n\n    b) The work must carry prominent notices stating that it is\n    released under this License and any conditions added under section\n    7.  This requirement modifies the requirement in section 4 to\n    \"keep intact all notices\".\n\n    c) You must license the entire work, as a whole, under this\n    License to anyone who comes into possession of a copy.  This\n    License will therefore apply, along with any applicable section 7\n    additional terms, to the whole of the work, and all its parts,\n    regardless of how they are packaged.  This License gives no\n    permission to license the work in any other way, but it does not\n    invalidate such permission if you have separately received it.\n\n    d) If the work has interactive user interfaces, each must display\n    Appropriate Legal Notices; however, if the Program has interactive\n    interfaces that do not display Appropriate Legal Notices, your\n    work need not make them do so.\n\n  A compilation of a covered work with other separate and independent\nworks, which are not by their nature extensions of the covered work,\nand which are not combined with it such as to form a larger program,\nin or on a volume of a storage or distribution medium, is called an\n\"aggregate\" if the compilation and its resulting copyright are not\nused to limit the access or legal rights of the compilation's users\nbeyond what the individual works permit.  Inclusion of a covered work\nin an aggregate does not cause this License to apply to the other\nparts of the aggregate.\n\n  6. Conveying Non-Source Forms.\n\n  You may convey a covered work in object code form under the terms\nof sections 4 and 5, provided that you also convey the\nmachine-readable Corresponding Source under the terms of this License,\nin one of these ways:\n\n    a) Convey the object code in, or embodied in, a physical product\n    (including a physical distribution medium), accompanied by the\n    Corresponding Source fixed on a durable physical medium\n    customarily used for software interchange.\n\n    b) Convey the object code in, or embodied in, a physical product\n    (including a physical distribution medium), accompanied by a\n    written offer, valid for at least three years and valid for as\n    long as you offer spare parts or customer support for that product\n    model, to give anyone who possesses the object code either (1) a\n    copy of the Corresponding Source for all the software in the\n    product that is covered by this License, on a durable physical\n    medium customarily used for software interchange, for a price no\n    more than your reasonable cost of physically performing this\n    conveying of source, or (2) access to copy the\n    Corresponding Source from a network server at no charge.\n\n    c) Convey individual copies of the object code with a copy of the\n    written offer to provide the Corresponding Source.  This\n    alternative is allowed only occasionally and noncommercially, and\n    only if you received the object code with such an offer, in accord\n    with subsection 6b.\n\n    d) Convey the object code by offering access from a designated\n    place (gratis or for a charge), and offer equivalent access to the\n    Corresponding Source in the same way through the same place at no\n    further charge.  You need not require recipients to copy the\n    Corresponding Source along with the object code.  If the place to\n    copy the object code is a network server, the Corresponding Source\n    may be on a different server (operated by you or a third party)\n    that supports equivalent copying facilities, provided you maintain\n    clear directions next to the object code saying where to find the\n    Corresponding Source.  Regardless of what server hosts the\n    Corresponding Source, you remain obligated to ensure that it is\n    available for as long as needed to satisfy these requirements.\n\n    e) Convey the object code using peer-to-peer transmission, provided\n    you inform other peers where the object code and Corresponding\n    Source of the work are being offered to the general public at no\n    charge under subsection 6d.\n\n  A separable portion of the object code, whose source code is excluded\nfrom the Corresponding Source as a System Library, need not be\nincluded in conveying the object code work.\n\n  A \"User Product\" is either (1) a \"consumer product\", which means any\ntangible personal property which is normally used for personal, family,\nor household purposes, or (2) anything designed or sold for incorporation\ninto a dwelling.  In determining whether a product is a consumer product,\ndoubtful cases shall be resolved in favor of coverage.  For a particular\nproduct received by a particular user, \"normally used\" refers to a\ntypical or common use of that class of product, regardless of the status\nof the particular user or of the way in which the particular user\nactually uses, or expects or is expected to use, the product.  A product\nis a consumer product regardless of whether the product has substantial\ncommercial, industrial or non-consumer uses, unless such uses represent\nthe only significant mode of use of the product.\n\n  \"Installation Information\" for a User Product means any methods,\nprocedures, authorization keys, or other information required to install\nand execute modified versions of a covered work in that User Product from\na modified version of its Corresponding Source.  The information must\nsuffice to ensure that the continued functioning of the modified object\ncode is in no case prevented or interfered with solely because\nmodification has been made.\n\n  If you convey an object code work under this section in, or with, or\nspecifically for use in, a User Product, and the conveying occurs as\npart of a transaction in which the right of possession and use of the\nUser Product is transferred to the recipient in perpetuity or for a\nfixed term (regardless of how the transaction is characterized), the\nCorresponding Source conveyed under this section must be accompanied\nby the Installation Information.  But this requirement does not apply\nif neither you nor any third party retains the ability to install\nmodified object code on the User Product (for example, the work has\nbeen installed in ROM).\n\n  The requirement to provide Installation Information does not include a\nrequirement to continue to provide support service, warranty, or updates\nfor a work that has been modified or installed by the recipient, or for\nthe User Product in which it has been modified or installed.  Access to a\nnetwork may be denied when the modification itself materially and\nadversely affects the operation of the network or violates the rules and\nprotocols for communication across the network.\n\n  Corresponding Source conveyed, and Installation Information provided,\nin accord with this section must be in a format that is publicly\ndocumented (and with an implementation available to the public in\nsource code form), and must require no special password or key for\nunpacking, reading or copying.\n\n  7. Additional Terms.\n\n  \"Additional permissions\" are terms that supplement the terms of this\nLicense by making exceptions from one or more of its conditions.\nAdditional permissions that are applicable to the entire Program shall\nbe treated as though they were included in this License, to the extent\nthat they are valid under applicable law.  If additional permissions\napply only to part of the Program, that part may be used separately\nunder those permissions, but the entire Program remains governed by\nthis License without regard to the additional permissions.\n\n  When you convey a copy of a covered work, you may at your option\nremove any additional permissions from that copy, or from any part of\nit.  (Additional permissions may be written to require their own\nremoval in certain cases when you modify the work.)  You may place\nadditional permissions on material, added by you to a covered work,\nfor which you have or can give appropriate copyright permission.\n\n  Notwithstanding any other provision of this License, for material you\nadd to a covered work, you may (if authorized by the copyright holders of\nthat material) supplement the terms of this License with terms:\n\n    a) Disclaiming warranty or limiting liability differently from the\n    terms of sections 15 and 16 of this License; or\n\n    b) Requiring preservation of specified reasonable legal notices or\n    author attributions in that material or in the Appropriate Legal\n    Notices displayed by works containing it; or\n\n    c) Prohibiting misrepresentation of the origin of that material, or\n    requiring that modified versions of such material be marked in\n    reasonable ways as different from the original version; or\n\n    d) Limiting the use for publicity purposes of names of licensors or\n    authors of the material; or\n\n    e) Declining to grant rights under trademark law for use of some\n    trade names, trademarks, or service marks; or\n\n    f) Requiring indemnification of licensors and authors of that\n    material by anyone who conveys the material (or modified versions of\n    it) with contractual assumptions of liability to the recipient, for\n    any liability that these contractual assumptions directly impose on\n    those licensors and authors.\n\n  All other non-permissive additional terms are considered \"further\nrestrictions\" within the meaning of section 10.  If the Program as you\nreceived it, or any part of it, contains a notice stating that it is\ngoverned by this License along with a term that is a further\nrestriction, you may remove that term.  If a license document contains\na further restriction but permits relicensing or conveying under this\nLicense, you may add to a covered work material governed by the terms\nof that license document, provided that the further restriction does\nnot survive such relicensing or conveying.\n\n  If you add terms to a covered work in accord with this section, you\nmust place, in the relevant source files, a statement of the\nadditional terms that apply to those files, or a notice indicating\nwhere to find the applicable terms.\n\n  Additional terms, permissive or non-permissive, may be stated in the\nform of a separately written license, or stated as exceptions;\nthe above requirements apply either way.\n\n  8. Termination.\n\n  You may not propagate or modify a covered work except as expressly\nprovided under this License.  Any attempt otherwise to propagate or\nmodify it is void, and will automatically terminate your rights under\nthis License (including any patent licenses granted under the third\nparagraph of section 11).\n\n  However, if you cease all violation of this License, then your\nlicense from a particular copyright holder is reinstated (a)\nprovisionally, unless and until the copyright holder explicitly and\nfinally terminates your license, and (b) permanently, if the copyright\nholder fails to notify you of the violation by some reasonable means\nprior to 60 days after the cessation.\n\n  Moreover, your license from a particular copyright holder is\nreinstated permanently if the copyright holder notifies you of the\nviolation by some reasonable means, this is the first time you have\nreceived notice of violation of this License (for any work) from that\ncopyright holder, and you cure the violation prior to 30 days after\nyour receipt of the notice.\n\n  Termination of your rights under this section does not terminate the\nlicenses of parties who have received copies or rights from you under\nthis License.  If your rights have been terminated and not permanently\nreinstated, you do not qualify to receive new licenses for the same\nmaterial under section 10.\n\n  9. Acceptance Not Required for Having Copies.\n\n  You are not required to accept this License in order to receive or\nrun a copy of the Program.  Ancillary propagation of a covered work\noccurring solely as a consequence of using peer-to-peer transmission\nto receive a copy likewise does not require acceptance.  However,\nnothing other than this License grants you permission to propagate or\nmodify any covered work.  These actions infringe copyright if you do\nnot accept this License.  Therefore, by modifying or propagating a\ncovered work, you indicate your acceptance of this License to do so.\n\n  10. Automatic Licensing of Downstream Recipients.\n\n  Each time you convey a covered work, the recipient automatically\nreceives a license from the original licensors, to run, modify and\npropagate that work, subject to this License.  You are not responsible\nfor enforcing compliance by third parties with this License.\n\n  An \"entity transaction\" is a transaction transferring control of an\norganization, or substantially all assets of one, or subdividing an\norganization, or merging organizations.  If propagation of a covered\nwork results from an entity transaction, each party to that\ntransaction who receives a copy of the work also receives whatever\nlicenses to the work the party's predecessor in interest had or could\ngive under the previous paragraph, plus a right to possession of the\nCorresponding Source of the work from the predecessor in interest, if\nthe predecessor has it or can get it with reasonable efforts.\n\n  You may not impose any further restrictions on the exercise of the\nrights granted or affirmed under this License.  For example, you may\nnot impose a license fee, royalty, or other charge for exercise of\nrights granted under this License, and you may not initiate litigation\n(including a cross-claim or counterclaim in a lawsuit) alleging that\nany patent claim is infringed by making, using, selling, offering for\nsale, or importing the Program or any portion of it.\n\n  11. Patents.\n\n  A \"contributor\" is a copyright holder who authorizes use under this\nLicense of the Program or a work on which the Program is based.  The\nwork thus licensed is called the contributor's \"contributor version\".\n\n  A contributor's \"essential patent claims\" are all patent claims\nowned or controlled by the contributor, whether already acquired or\nhereafter acquired, that would be infringed by some manner, permitted\nby this License, of making, using, or selling its contributor version,\nbut do not include claims that would be infringed only as a\nconsequence of further modification of the contributor version.  For\npurposes of this definition, \"control\" includes the right to grant\npatent sublicenses in a manner consistent with the requirements of\nthis License.\n\n  Each contributor grants you a non-exclusive, worldwide, royalty-free\npatent license under the contributor's essential patent claims, to\nmake, use, sell, offer for sale, import and otherwise run, modify and\npropagate the contents of its contributor version.\n\n  In the following three paragraphs, a \"patent license\" is any express\nagreement or commitment, however denominated, not to enforce a patent\n(such as an express permission to practice a patent or covenant not to\nsue for patent infringement).  To \"grant\" such a patent license to a\nparty means to make such an agreement or commitment not to enforce a\npatent against the party.\n\n  If you convey a covered work, knowingly relying on a patent license,\nand the Corresponding Source of the work is not available for anyone\nto copy, free of charge and under the terms of this License, through a\npublicly available network server or other readily accessible means,\nthen you must either (1) cause the Corresponding Source to be so\navailable, or (2) arrange to deprive yourself of the benefit of the\npatent license for this particular work, or (3) arrange, in a manner\nconsistent with the requirements of this License, to extend the patent\nlicense to downstream recipients.  \"Knowingly relying\" means you have\nactual knowledge that, but for the patent license, your conveying the\ncovered work in a country, or your recipient's use of the covered work\nin a country, would infringe one or more identifiable patents in that\ncountry that you have reason to believe are valid.\n\n  If, pursuant to or in connection with a single transaction or\narrangement, you convey, or propagate by procuring conveyance of, a\ncovered work, and grant a patent license to some of the parties\nreceiving the covered work authorizing them to use, propagate, modify\nor convey a specific copy of the covered work, then the patent license\nyou grant is automatically extended to all recipients of the covered\nwork and works based on it.\n\n  A patent license is \"discriminatory\" if it does not include within\nthe scope of its coverage, prohibits the exercise of, or is\nconditioned on the non-exercise of one or more of the rights that are\nspecifically granted under this License.  You may not convey a covered\nwork if you are a party to an arrangement with a third party that is\nin the business of distributing software, under which you make payment\nto the third party based on the extent of your activity of conveying\nthe work, and under which the third party grants, to any of the\nparties who would receive the covered work from you, a discriminatory\npatent license (a) in connection with copies of the covered work\nconveyed by you (or copies made from those copies), or (b) primarily\nfor and in connection with specific products or compilations that\ncontain the covered work, unless you entered into that arrangement,\nor that patent license was granted, prior to 28 March 2007.\n\n  Nothing in this License shall be construed as excluding or limiting\nany implied license or other defenses to infringement that may\notherwise be available to you under applicable patent law.\n\n  12. No Surrender of Others' Freedom.\n\n  If conditions are imposed on you (whether by court order, agreement or\notherwise) that contradict the conditions of this License, they do not\nexcuse you from the conditions of this License.  If you cannot convey a\ncovered work so as to satisfy simultaneously your obligations under this\nLicense and any other pertinent obligations, then as a consequence you may\nnot convey it at all.  For example, if you agree to terms that obligate you\nto collect a royalty for further conveying from those to whom you convey\nthe Program, the only way you could satisfy both those terms and this\nLicense would be to refrain entirely from conveying the Program.\n\n  13. Use with the GNU Affero General Public License.\n\n  Notwithstanding any other provision of this License, you have\npermission to link or combine any covered work with a work licensed\nunder version 3 of the GNU Affero General Public License into a single\ncombined work, and to convey the resulting work.  The terms of this\nLicense will continue to apply to the part which is the covered work,\nbut the special requirements of the GNU Affero General Public License,\nsection 13, concerning interaction through a network will apply to the\ncombination as such.\n\n  14. Revised Versions of this License.\n\n  The Free Software Foundation may publish revised and/or new versions of\nthe GNU General Public License from time to time.  Such new versions will\nbe similar in spirit to the present version, but may differ in detail to\naddress new problems or concerns.\n\n  Each version is given a distinguishing version number.  If the\nProgram specifies that a certain numbered version of the GNU General\nPublic License \"or any later version\" applies to it, you have the\noption of following the terms and conditions either of that numbered\nversion or of any later version published by the Free Software\nFoundation.  If the Program does not specify a version number of the\nGNU General Public License, you may choose any version ever published\nby the Free Software Foundation.\n\n  If the Program specifies that a proxy can decide which future\nversions of the GNU General Public License can be used, that proxy's\npublic statement of acceptance of a version permanently authorizes you\nto choose that version for the Program.\n\n  Later license versions may give you additional or different\npermissions.  However, no additional obligations are imposed on any\nauthor or copyright holder as a result of your choosing to follow a\nlater version.\n\n  15. Disclaimer of Warranty.\n\n  THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY\nAPPLICABLE LAW.  EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT\nHOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM \"AS IS\" WITHOUT WARRANTY\nOF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO,\nTHE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR\nPURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM\nIS WITH YOU.  SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF\nALL NECESSARY SERVICING, REPAIR OR CORRECTION.\n\n  16. Limitation of Liability.\n\n  IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING\nWILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS\nTHE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY\nGENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE\nUSE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF\nDATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD\nPARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS),\nEVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF\nSUCH DAMAGES.\n\n  17. Interpretation of Sections 15 and 16.\n\n  If the disclaimer of warranty and limitation of liability provided\nabove cannot be given local legal effect according to their terms,\nreviewing courts shall apply local law that most closely approximates\nan absolute waiver of all civil liability in connection with the\nProgram, unless a warranty or assumption of liability accompanies a\ncopy of the Program in return for a fee.\n\n                     END OF TERMS AND CONDITIONS\n\n            How to Apply These Terms to Your New Programs\n\n  If you develop a new program, and you want it to be of the greatest\npossible use to the public, the best way to achieve this is to make it\nfree software which everyone can redistribute and change under these terms.\n\n  To do so, attach the following notices to the program.  It is safest\nto attach them to the start of each source file to most effectively\nstate the exclusion of warranty; and each file should have at least\nthe \"copyright\" line and a pointer to where the full notice is found.\n\n    <one line to give the program's name and a brief idea of what it does.>\n    Copyright (C) <year>  <name of author>\n\n    This program is free software: you can redistribute it and/or modify\n    it under the terms of the GNU General Public License as published by\n    the Free Software Foundation, either version 3 of the License, or\n    (at your option) any later version.\n\n    This program is distributed in the hope that it will be useful,\n    but WITHOUT ANY WARRANTY; without even the implied warranty of\n    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n    GNU General Public License for more details.\n\n    You should have received a copy of the GNU General Public License\n    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n\nAlso add information on how to contact you by electronic and paper mail.\n\n  If the program does terminal interaction, make it output a short\nnotice like this when it starts in an interactive mode:\n\n    <program>  Copyright (C) <year>  <name of author>\n    This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'.\n    This is free software, and you are welcome to redistribute it\n    under certain conditions; type `show c' for details.\n\nThe hypothetical commands `show w' and `show c' should show the appropriate\nparts of the General Public License.  Of course, your program's commands\nmight be different; for a GUI interface, you would use an \"about box\".\n\n  You should also get your employer (if you work as a programmer) or school,\nif any, to sign a \"copyright disclaimer\" for the program, if necessary.\nFor more information on this, and how to apply and follow the GNU GPL, see\n<https://www.gnu.org/licenses/>.\n\n  The GNU General Public License does not permit incorporating your program\ninto proprietary programs.  If your program is a subroutine library, you\nmay consider it more useful to permit linking proprietary applications with\nthe library.  If this is what you want to do, use the GNU Lesser General\nPublic License instead of this License.  But first, please read\n<https://www.gnu.org/licenses/why-not-lgpl.html>.\n"
  },
  {
    "path": "camera_model/include/MeiCamera.h",
    "content": "/**   This File is part of Camera Model\r\n *  \r\n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\r\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\r\n *    \r\n *    This program is free software: you can redistribute it and/or modify\r\n *    it under the terms of the GNU General Public License as published by\r\n *    the Free Software Foundation, either version 3 of the License, or\r\n *    (at your option) any later version.\r\n *    \r\n *    This program is distributed in the hope that it will be useful,\r\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\r\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\r\n *    GNU General Public License for more details.\r\n *    \r\n *    You should have received a copy of the GNU General Public License\r\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\r\n *    \r\n *    Since some functions are adapted from GVINS <https://github.com/HKUST-Aerial-Robotics/GVINS>\r\n *    GVINS is under GPLv3 License.\r\n */\r\n\r\n#pragma once\r\n\r\n#include \"camera.h\"\r\n#include <cmath>\r\n\r\nnamespace camera_model\r\n{\r\n    class MeiCamera: public Camera\r\n    {\r\n    public:\r\n        class Parameters: public Camera::Parameters\r\n        {\r\n        public:\r\n            Parameters();\r\n            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);\r\n\r\n            double& xi();\r\n            double& k1();\r\n            double& k2();\r\n            double& p1();\r\n            double& p2();\r\n            double& gamma1();\r\n            double& gamma2();\r\n            double& u0();\r\n            double& v0();\r\n\r\n            double xi() const;\r\n            double k1() const;\r\n            double k2() const;\r\n            double p1() const;\r\n            double p2() const;\r\n            double gamma1() const;\r\n            double gamma2() const;\r\n            double u0() const;\r\n            double v0() const;\r\n            \r\n            bool readFromYamlFile(const std::string& filename) override;\r\n            void writeToYamlFile(const std::string& filename) const override;\r\n            \r\n            Parameters& operator= (const Parameters& other);\r\n            friend std::ostream& operator<< (std::ostream& out, const Parameters& params);\r\n            \r\n        private:\r\n            double m_xi;\r\n            double m_k1;\r\n            double m_k2;\r\n            double m_p1;\r\n            double m_p2;\r\n            double m_gamma1;\r\n            double m_gamma2;\r\n            double m_u0;\r\n            double m_v0;\r\n        };\r\n        \r\n        MeiCamera();\r\n        \r\n        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);\r\n        \r\n        MeiCamera(const Parameters& params);\r\n        \r\n        Camera::ModelType modelType(void) const override;\r\n\r\n        const std::string& cameraName(void) const override;\r\n\r\n        int imageWidth(void) const override;\r\n\r\n        int imageHeight(void) const override;\r\n        \r\n        double getNormalPixel() const override;\r\n       \r\n        void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const override;\r\n        \r\n        void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const override;\r\n        \r\n        void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const override;\r\n        \r\n        void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const override;\r\n        \r\n        template <typename T>\r\n        static void spaceToPlane(const T* const params, const T* const q, const T* const t, const Eigen::Matrix<T, 3, 1>& P, Eigen::Matrix<T, 2, 1>& p);\r\n        \r\n        void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const;\r\n        void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u, Eigen::Matrix2d& J) const;\r\n        \r\n        int parameterCount() const override;\r\n        \r\n        const Parameters& getParameters(void) const;\r\n\r\n        void setParameters(const Parameters& parameters);  \r\n        \r\n        void readParameters(const std::vector<double>& parameterVec) override;\r\n\r\n        void writeParameters(std::vector<double>& parameterVec) const override;\r\n\r\n        void writeParametersToYamlFile(const std::string& filename) const override;\r\n\r\n        std::string parametersToString(void) const override;\r\n        \r\n    private:\r\n        Parameters mParameters;\r\n        \r\n        double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23;\r\n        bool m_noDistortion;\r\n    };\r\n    \r\n    typedef boost::shared_ptr<MeiCamera> MeiCameraPtr;\r\n    typedef boost::shared_ptr<const MeiCamera> MeiCameraConstPtr;\r\n    \r\n    template <typename T>\r\n    void MeiCamera::spaceToPlane(const T* const params, const T* const q, const T* const t, const Eigen::Matrix<T, 3, 1>& P, Eigen::Matrix<T, 2, 1>& p)\r\n    {\r\n        Eigen::Vector3d Pw((double)P(0), (double)P(1), (double)P(2));\r\n        Eigen::Vector3d tw((double)t(0), (double)t(1), (double)t(2));\r\n        Eigen::Quaterniond qw;\r\n        \r\n        qw.w() = (double) q[3];\r\n        qw.x() = (double) q[0];\r\n        qw.y() = (double) q[1];\r\n        qw.z() = (double) q[2];\r\n        qw.normalized();\r\n        \r\n        Eigen::Vector3d transP = qw.toRotationMatrix()*Pw + tw;\r\n        T P_c[3];\r\n        P_c[0] = T(transP(0));\r\n        P_c[1] = T(transP(1));\r\n        P_c[2] = T(transP(2));\r\n        \r\n        T xi = params[0];\r\n        T k1 = params[1];\r\n        T k2 = params[2];\r\n        T p1 = params[3];\r\n        T p2 = params[4];\r\n\r\n        T gamma1 = params[5];\r\n        T gamma2 = params[6];\r\n        T alpha = T(0);\r\n        T u0 = params[7];\r\n        T v0 = params[8];\r\n        \r\n        T len = std::sqrt(P_c[0]*P_c[0] + P_c[1]*P_c[1] + P_c[2]*P_c[2]);\r\n        P_c[0] /= len;\r\n        P_c[1] /= len;\r\n        P_c[2] /= len;\r\n        \r\n        T u = P_c[0]/(P_c[2] + xi);\r\n        T v = P_c[1]/(P_c[2] + xi);\r\n        \r\n        T rho_sqr = u*u + v*v;\r\n        T L = T(1.0) + k1*rho_sqr + k2*rho_sqr*rho_sqr;\r\n        T du = T(2.0)*p1*u*v + p2*(rho_sqr + T(2.0)*u*u);\r\n        T dv = p1*(rho_sqr + T(2.0)*v*v) + T(2.0)*p2*u*v;\r\n\r\n        u = L*u + du;\r\n        v = L*v + dv;\r\n\r\n        p(0) = gamma1*(u + alpha*v) + u0;\r\n        p(1) = gamma2*v + v0;        \r\n    }\r\n}\r\n"
  },
  {
    "path": "camera_model/include/PinholeCamera.h",
    "content": "/**   This File is part of Camera Model\n *  \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n *    \n *    Since some functions are adapted from GVINS <https://github.com/HKUST-Aerial-Robotics/GVINS>\n *    GVINS is under GPLv3 License.\n */\n\n#pragma once\n\n#include \"camera.h\"\n#include <cmath>\n\nnamespace camera_model\n{\n    class PinholeCamera: public Camera\n    {\n    public:\n        class Parameters: public Camera::Parameters\n        {\n        public:\n            Parameters();\n            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);\n\n            double& k1(void);\n            double& k2(void);\n            double& p1(void);\n            double& p2(void);\n            double& fx(void);\n            double& fy(void);\n            double& cx(void);\n            double& cy(void);\n\n            double xi(void) const;\n            double k1(void) const;\n            double k2(void) const;\n            double p1(void) const;\n            double p2(void) const;\n            double fx(void) const;\n            double fy(void) const;\n            double cx(void) const;\n            double cy(void) const;\n\n            bool readFromYamlFile(const std::string& filename) override;\n            void writeToYamlFile(const std::string& filename) const override;\n\n            Parameters& operator=(const Parameters& other);\n            friend std::ostream& operator<< (std::ostream& out, const Parameters& params);\n\n        private:\n            double m_k1;\n            double m_k2;\n            double m_p1;\n            double m_p2;\n            double m_fx;\n            double m_fy;\n            double m_cx;\n            double m_cy;\n        };\n        \n        PinholeCamera();\n        \n        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);\n        \n        PinholeCamera(const Parameters& params);\n        \n        Camera::ModelType modelType(void) const override;\n        const std::string& cameraName(void) const override;\n        int imageWidth(void) const override;\n        int imageHeight(void) const override;\n        \n        double getNormalPixel() const override;\n        \n        void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const override;\n\n        void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const override;\n\n        void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const override;\n\n        void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p, Eigen::Matrix<double,2,3>& J) const;\n        \n        void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const override;\n        \n        template <typename T>\n        static void spaceToPlane(const T* const params, const T* const q, const T* const t, const Eigen::Matrix<T, 3, 1>& P, Eigen::Matrix<T, 2, 1>& p);\n        \n        void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const;\n        void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u, Eigen::Matrix2d& J) const;\n        \n        int parameterCount() const override;\n        \n        const Parameters& getParameters(void) const;\n\n        void setParameters(const Parameters& parameters);  \n        \n        void readParameters(const std::vector<double>& parameterVec) override;\n\n        void writeParameters(std::vector<double>& parameterVec) const override;\n\n        void writeParametersToYamlFile(const std::string& filename) const override;\n\n        std::string parametersToString(void) const override;\n        \n    private:\n        Parameters mParameters;\n        \n        double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23;\n        bool m_noDistortion;\n    };\n    \n    typedef boost::shared_ptr<PinholeCamera> PinholeCameraPtr;\n    typedef boost::shared_ptr<const PinholeCamera> PinholeCameraConstPtr;\n    \n    template <typename T>\n    void PinholeCamera::spaceToPlane(const T* const params, const T* const q, const T* const t, const Eigen::Matrix<T, 3, 1>& P, Eigen::Matrix<T, 2, 1>& p)\n    {\n        Eigen::Vector3d Pw((double)P(0), (double)P(1), (double)P(2));\n        Eigen::Vector3d tw((double)t(0), (double)t(1), (double)t(2));\n        Eigen::Quaterniond qw;\n        \n        qw.w() = (double) q[3];\n        qw.x() = (double) q[0];\n        qw.y() = (double) q[1];\n        qw.z() = (double) q[2];\n        qw.normalized();\n        \n        Eigen::Vector3d transP = qw.toRotationMatrix()*Pw + tw;\n        T P_c[3];\n        P_c[0] = T(transP(0));\n        P_c[1] = T(transP(1));\n        P_c[2] = T(transP(2));\n        \n        T k1 = params[0];\n        T k2 = params[1];\n        T p1 = params[2];\n        T p2 = params[3];\n        T fx = params[4];\n        T fy = params[5];\n        T alpha = T(0);\n        T cx = params[6];\n        T cy = params[7];\n\n        T u = P_c[0]/P_c[2];\n        T v = P_c[1]/P_c[2];\n\n        T rho_sqr = u*u + v*v;\n        T L = T(1.0) + k1*rho_sqr + k2*rho_sqr*rho_sqr;\n        T du = T(2.0)*p1*u*v + p2*(rho_sqr + T(2.0)*u*u);\n        T dv = p1*(rho_sqr + T(2.0)*v*v) + T(2.0)*p2*u*v;\n\n        u = L*u + du;\n        v = L*v + dv;\n        p(0) = fx*(u + alpha*v) + cx;\n        p(1) = fy*v + cy;\n    }\n    \n}\n"
  },
  {
    "path": "camera_model/include/camera.h",
    "content": "/**   This File is part Camera Model\n *  \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n *    \n *    Since some functions are adapted from GVINS <https://github.com/HKUST-Aerial-Robotics/GVINS>\n *    GVINS is under GPLv3 License.\n */\n\n#pragma once\n\n#include <boost/shared_ptr.hpp>\n#include <Eigen/Dense>\n#include <Eigen/Geometry>\n#include <vector>\n#include <opencv2/core/core.hpp>\n#include <string>\n#include <iostream>\n\nnamespace camera_model\n{\n    class Camera\n    {\n    public:\n        EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n        \n        enum ModelType\n        {\n            MEI,\n            PINHOLE\n        };\n        \n        class Parameters\n        {\n        public:\n            EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n            \n            Parameters(ModelType modelType);\n            Parameters(ModelType modelType, const std::string& cameraName, int w, int h);\n            \n            ModelType& modelType();\n            ModelType modelType() const;\n            \n            std::string& cameraName();\n            const std::string& cameraName() const;\n            \n            int& imageWidth();\n            int imageWidth() const;\n            \n            int& imageHeight();\n            int imageHeight() const;\n            \n            int nIntrinsics() const;\n            \n            virtual bool readFromYamlFile(const std::string& filename) = 0;\n            virtual void writeToYamlFile(const std::string& filename) const = 0;\n            \n        protected:\n            ModelType m_modelType;\n            int m_nIntrinsics;\n            std::string m_cameraName;\n            int m_imageWidth;\n            int m_imageHeight;\n        };\n        \n        \n        virtual ModelType modelType() const  = 0;\n        virtual const std::string& cameraName() const = 0;\n        virtual int imageWidth() const = 0;\n        virtual int imageHeight() const = 0;\n        \n        virtual cv::Mat& mask();\n        virtual const cv::Mat& mask() const;\n        \n        virtual double getNormalPixel() const = 0;\n        \n        // from the image plane to the sphere\n        virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const = 0;\n        \n        // from the image plane to the projective space\n        virtual void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const = 0;\n        \n        // from 3D points to the image plane\n        virtual void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const = 0;\n        \n        virtual void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const = 0;\n        \n        virtual int parameterCount(void) const = 0;\n        \n        virtual void readParameters(const std::vector<double>& parameters) = 0;\n        virtual void writeParameters(std::vector<double>& parameters) const = 0;\n        virtual void writeParametersToYamlFile(const std::string& filename) const = 0;\n        virtual std::string parametersToString() const = 0;\n        \n        double reprojectionDist(const Eigen::Vector3d& P1, const Eigen::Vector3d& P2) const;\n\n        double reprojectionError(const Eigen::Vector3d& P,\n                                 const Eigen::Quaterniond& camera_q,\n                                 const Eigen::Vector3d& camera_t,\n                                 const Eigen::Vector2d& observed_p) const;\n                           \n    protected:\n        cv::Mat m_mask;\n    };\n    \n    typedef boost::shared_ptr<Camera> CameraPtr;\n    typedef boost::shared_ptr<const Camera> CameraConstPtr;\n}\n"
  },
  {
    "path": "camera_model/include/camera_factory.h",
    "content": "/**   This File is part of Camera Model\n *  \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n *    \n *    Since some functions are adapted from GVINS <https://github.com/HKUST-Aerial-Robotics/GVINS>\n *    GVINS is under GPLv3 License.\n */\n\n#pragma once\n\n#include \"camera.h\"\n\nnamespace camera_model\n{\n    class CameraFactory\n    {\n    public:\n        EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n        \n        CameraFactory();\n        \n        static boost::shared_ptr<CameraFactory> instance(void);\n        \n        CameraPtr generateCamera(Camera::ModelType modelType, const std::string& cameraName, cv::Size imageSize) const;\n        \n        CameraPtr generateCameraFromYamlFile(const std::string& filename);\n        \n    private:\n        static boost::shared_ptr<CameraFactory> m_instance;\n    };\n    \n}\n"
  },
  {
    "path": "camera_model/package.xml",
    "content": "<?xml version=\"1.0\"?>\n<package>\n  <name>camera_model</name>\n  <version>0.0.0</version>\n  <description>The camera_model package</description>\n  <maintainer email=\"lcw18@mails.tsinghua.edu.cn\">lcw18</maintainer>\n  <!-- One license tag required, multiple allowed, one license per tag -->\n  <!-- Commonly used license strings: -->\n  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->\n  <license>GPLv3</license>\n\n  <!-- The *_depend tags are used to specify dependencies -->\n  <!-- Dependencies can be catkin packages or system dependencies -->\n  <!-- Examples: -->\n  <!-- Use build_depend for packages you need at compile time: -->\n  <!--   <build_depend>message_generation</build_depend> -->\n  <!-- Use buildtool_depend for build tool packages: -->\n  <!--   <buildtool_depend>catkin</buildtool_depend> -->\n  <!-- Use run_depend for packages you need at runtime: -->\n  <!--   <run_depend>message_runtime</run_depend> -->\n  <!-- Use test_depend for packages you need only for testing: -->\n  <!--   <test_depend>gtest</test_depend> -->\n  <buildtool_depend>catkin</buildtool_depend>\n  <build_depend>roscpp</build_depend>\n  <build_depend>std_msgs</build_depend>\n  <run_depend>roscpp</run_depend>\n  <run_depend>std_msgs</run_depend>\n\n  <!-- The export tag contains other, unspecified, tags -->\n  <export>\n    <!-- Other tools can request additional information be placed here -->\n  </export>\n</package>\n"
  },
  {
    "path": "camera_model/src/MeiCamera.cpp",
    "content": "/**   This File is part of Camera Model\n *  \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n *    \n *    Since some functions are adapted from GVINS <https://github.com/HKUST-Aerial-Robotics/GVINS>\n *    GVINS is under GPLv3 License.\n */\n\n#include \"MeiCamera.h\"\n#include <iomanip>\n\nnamespace camera_model\n{\n    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)\n    {}\n    \n    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)\n    {}\n    \n    double& MeiCamera::Parameters::xi() { return m_xi;}\n\n    double& MeiCamera::Parameters::k1() { return m_k1;}\n\n    double& MeiCamera::Parameters::k2() { return m_k2;}\n\n    double& MeiCamera::Parameters::p1() { return m_p1;}\n\n    double& MeiCamera::Parameters::p2() { return m_p2;}\n\n    double& MeiCamera::Parameters::gamma1() { return m_gamma1;}\n\n    double& MeiCamera::Parameters::gamma2() { return m_gamma2;}\n\n    double& MeiCamera::Parameters::u0() { return m_u0;}\n\n    double& MeiCamera::Parameters::v0() { return m_v0;}\n\n    double MeiCamera::Parameters::xi() const { return m_xi;}\n\n    double MeiCamera::Parameters::k1() const { return m_k1;}\n\n    double MeiCamera::Parameters::k2() const { return m_k2;}\n\n    double MeiCamera::Parameters::p1() const { return m_p1;}\n\n    double MeiCamera::Parameters::p2() const { return m_p2;}\n\n    double MeiCamera::Parameters::gamma1() const { return m_gamma1;}\n\n    double MeiCamera::Parameters::gamma2() const { return m_gamma2;}\n\n    double MeiCamera::Parameters::u0() const { return m_u0;}\n\n    double MeiCamera::Parameters::v0() const { return m_v0;}\n\n    bool MeiCamera::Parameters::readFromYamlFile(const std::string& filename)\n    {\n        cv::FileStorage fs(filename, cv::FileStorage::READ);\n        \n        if (!fs.isOpened()) return false;\n        \n        if (!fs[\"model_type\"].isNone())\n        {\n            std::string sModelType;\n            fs[\"model_type\"] >> sModelType;\n            \n            if (sModelType.compare(\"MEI\") != 0) return false;\n        }\n        \n        m_modelType = MEI;\n        fs[\"camera_name\"] >> m_cameraName;\n        m_imageWidth = static_cast<int>(fs[\"image_width\"]);\n        m_imageHeight = static_cast<int>(fs[\"image_height\"]);\n\n        cv::FileNode n = fs[\"mirror_parameters\"];\n        m_xi = static_cast<double>(n[\"xi\"]);\n\n        n = fs[\"distortion_parameters\"];\n        m_k1 = static_cast<double>(n[\"k1\"]);\n        m_k2 = static_cast<double>(n[\"k2\"]);\n        m_p1 = static_cast<double>(n[\"p1\"]);\n        m_p2 = static_cast<double>(n[\"p2\"]);\n        \n        n = fs[\"projection_parameters\"];\n        m_gamma1 = static_cast<double>(n[\"gamma1\"]);\n        m_gamma2 = static_cast<double>(n[\"gamma2\"]);\n        m_u0 = static_cast<double>(n[\"u0\"]);\n        m_v0 = static_cast<double>(n[\"v0\"]);\n    \n        return true;\n    }\n    \n    void MeiCamera::Parameters::writeToYamlFile(const std::string& filename) const\n    {\n        cv::FileStorage fs(filename, cv::FileStorage::WRITE);\n\n        fs << \"model_type\" << \"MEI\";\n        fs << \"camera_name\" << m_cameraName;\n        fs << \"image_width\" << m_imageWidth;\n        fs << \"image_height\" << m_imageHeight;\n\n        fs << \"mirror_parameters\";\n        fs << \"{\" << \"xi\" << m_xi << \"}\";\n    \n        fs << \"distortion_parameters\";\n        fs << \"{\" << \"k1\" << m_k1\n                  << \"k2\" << m_k2\n                  << \"p1\" << m_p1\n                  << \"p2\" << m_p2 << \"}\";\n        \n        fs << \"projection_parameters\";\n        fs << \"{\" << \"gamma1\" << m_gamma1\n                  << \"gamma2\" << m_gamma2\n                  << \"u0\" << m_u0\n                  << \"v0\" << m_v0 << \"}\";\n\n        fs.release();\n    }\n    \n    MeiCamera::Parameters& MeiCamera::Parameters::operator=(const MeiCamera::Parameters& other)\n    {\n        if (this != &other)\n        {\n            m_modelType = other.m_modelType;\n            m_cameraName = other.m_cameraName;\n            m_imageWidth = other.m_imageWidth;\n            m_imageHeight = other.m_imageHeight;\n            m_xi = other.m_xi;\n            m_k1 = other.m_k1;\n            m_k2 = other.m_k2;\n            m_p1 = other.m_p1;\n            m_p2 = other.m_p2;\n            m_gamma1 = other.m_gamma1;\n            m_gamma2 = other.m_gamma2;\n            m_u0 = other.m_u0;\n            m_v0 = other.m_v0;\n        }\n    \n        return *this;\n    }\n    \n    std::ostream& operator<< (std::ostream& out, const MeiCamera::Parameters& params)\n    {\n        out << \"Camera Parameters:\" << std::endl;\n        out << \"    model_type \" << \"MEI\" << std::endl;\n        out << \"   camera_name \" << params.m_cameraName << std::endl;\n        out << \"   image_width \" << params.m_imageWidth << std::endl;\n        out << \"  image_height \" << params.m_imageHeight << std::endl;\n\n        out << \"Mirror Parameters\" << std::endl;\n        out << std::fixed << std::setprecision(10);\n        out << \"            xi \" << params.m_xi << std::endl;\n\n        out << \"Distortion Parameters\" << std::endl;\n        out << \"            k1 \" << params.m_k1 << std::endl\n            << \"            k2 \" << params.m_k2 << std::endl\n            << \"            p1 \" << params.m_p1 << std::endl\n            << \"            p2 \" << params.m_p2 << std::endl;\n\n        out << \"Projection Parameters\" << std::endl;\n        out << \"        gamma1 \" << params.m_gamma1 << std::endl\n            << \"        gamma2 \" << params.m_gamma2 << std::endl\n            << \"            u0 \" << params.m_u0 << std::endl\n            << \"            v0 \" << params.m_v0 << std::endl;\n\n        return out;\n    }\n    \n    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)\n    {}\n    \n    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)\n    {\n        if ((mParameters.k1() == 0.0) && (mParameters.k2() == 0.0) && (mParameters.p1() == 0.0) && (mParameters.p2() == 0.0))\n        {\n            m_noDistortion = true;\n        }\n        else\n        {\n            m_noDistortion = false;\n        }\n        \n        m_inv_K11 = 1.0 / mParameters.gamma1();\n        m_inv_K13 = -mParameters.u0() / mParameters.gamma1();\n        m_inv_K22 = 1.0 / mParameters.gamma2();\n        m_inv_K23 = -mParameters.v0() / mParameters.gamma2();\n    }\n    \n    MeiCamera::MeiCamera(const MeiCamera::Parameters& params) : mParameters(params)\n    {\n        if ((mParameters.k1() == 0.0) && (mParameters.k2() == 0.0) && (mParameters.p1() == 0.0) && (mParameters.p2() == 0.0))\n        {\n            m_noDistortion = true;\n        }\n        else\n        {\n            m_noDistortion = false;\n        }\n        \n        m_inv_K11 = 1.0 / mParameters.gamma1();\n        m_inv_K13 = -mParameters.u0() / mParameters.gamma1();\n        m_inv_K22 = 1.0 / mParameters.gamma2();\n        m_inv_K23 = -mParameters.v0() / mParameters.gamma2();\n    }\n    \n    Camera::ModelType MeiCamera::modelType() const\n    {\n        return mParameters.modelType();\n    }\n\n    const std::string& MeiCamera::cameraName() const\n    {\n        return mParameters.cameraName();\n    }\n\n    int MeiCamera::imageWidth() const\n    {\n        return mParameters.imageWidth();\n    }\n\n    int MeiCamera::imageHeight() const\n    {\n        return mParameters.imageHeight();\n    }\n    \n    double MeiCamera::getNormalPixel() const\n    {\n        return 2.0/(mParameters.gamma1() + mParameters.gamma2());\n    }\n    \n    void MeiCamera::liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const\n    {\n        double mx_d, my_d,mx2_d, mxy_d, my2_d, mx_u, my_u;\n        double rho2_d, rho4_d, radDist_d, Dx_d, Dy_d, inv_denom_d;\n        double lambda;\n        \n        mx_d = m_inv_K11 * p(0) + m_inv_K13;\n        my_d = m_inv_K22 * p(1) + m_inv_K23;\n        \n        if (m_noDistortion)\n        {\n            mx_u = mx_d;\n            my_u = my_d;\n        }\n        else\n        {\n            double k1 = mParameters.k1();\n            double k2 = mParameters.k2();\n            double p1 = mParameters.p1();\n            double p2 = mParameters.p2();\n            \n            mx2_d = mx_d*mx_d;\n            my2_d = my_d*my_d;\n            mxy_d = mx_d*my_d;\n            rho2_d = mx2_d+my2_d;\n            rho4_d = rho2_d*rho2_d;\n            radDist_d = k1*rho2_d+k2*rho4_d;\n            Dx_d = mx_d*radDist_d + p2*(rho2_d+2*mx2_d) + 2*p1*mxy_d;\n            Dy_d = my_d*radDist_d + p1*(rho2_d+2*my2_d) + 2*p2*mxy_d;\n            inv_denom_d = 1/(1+4*k1*rho2_d+6*k2*rho4_d+8*p1*my_d+8*p2*mx_d);\n\n            mx_u = mx_d - inv_denom_d*Dx_d;\n            my_u = my_d - inv_denom_d*Dy_d;\n        }\n        \n        double xi = mParameters.xi();\n        if (xi == 1.0)\n        {\n            lambda = 2.0/(mx_u*mx_u + my_u*my_u + 1.0);\n            P << lambda*mx_u, lambda*my_u, lambda - 1.0;\n        }\n        else\n        {\n            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);\n            P << lambda*mx_u, lambda*my_u, lambda - xi;\n        }\n        \n    }\n    \n    void MeiCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const\n    {\n        double mx_d, my_d,mx2_d, mxy_d, my2_d, mx_u, my_u;\n        double rho2_d, rho4_d, radDist_d, Dx_d, Dy_d, inv_denom_d;\n\n        mx_d = m_inv_K11 * p(0) + m_inv_K13;\n        my_d = m_inv_K22 * p(1) + m_inv_K23;\n\n        if (m_noDistortion)\n        {\n            mx_u = mx_d;\n            my_u = my_d;\n        }\n        else\n        {\n            double k1 = mParameters.k1();\n            double k2 = mParameters.k2();\n            double p1 = mParameters.p1();\n            double p2 = mParameters.p2();\n\n            mx2_d = mx_d*mx_d;\n            my2_d = my_d*my_d;\n            mxy_d = mx_d*my_d;\n            rho2_d = mx2_d+my2_d;\n            rho4_d = rho2_d*rho2_d;\n            radDist_d = k1*rho2_d+k2*rho4_d;\n            Dx_d = mx_d*radDist_d + p2*(rho2_d+2*mx2_d) + 2*p1*mxy_d;\n            Dy_d = my_d*radDist_d + p1*(rho2_d+2*my2_d) + 2*p2*mxy_d;\n            inv_denom_d = 1/(1+4*k1*rho2_d+6*k2*rho4_d+8*p1*my_d+8*p2*mx_d);\n\n            mx_u = mx_d - inv_denom_d*Dx_d;\n            my_u = my_d - inv_denom_d*Dy_d;\n        }\n\n        double xi = mParameters.xi();\n        if (xi == 1.0)\n        {\n            P << mx_u, my_u, (1.0 - mx_u*mx_u - my_u*my_u)/2.0;\n        }\n        else\n        {\n            rho2_d = mx_u*mx_u + my_u*my_u;\n            P << mx_u, my_u, 1.0 - xi*(rho2_d + 1.0)/(xi + std::sqrt(1.0 + (1.0 - xi*xi)*rho2_d));\n        }\n        \n        P(0) = P(0)/P(2);\n        P(1) = P(1)/P(2);\n        P(2) = 1.0;\n    }\n    \n    void MeiCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const\n    {\n        Eigen::Vector2d p_u, p_d;\n\n        double z = P(2) + mParameters.xi() * P.norm();\n        p_u << P(0) / z, P(1) / z;\n\n        if (m_noDistortion)\n        {\n            p_d = p_u;\n        }\n        else\n        {\n            Eigen::Vector2d d_u;\n            distortion(p_u, d_u);\n            p_d = p_u + d_u;\n        }\n\n        p << mParameters.gamma1()*p_d(0) + mParameters.u0(),\n             mParameters.gamma2()*p_d(1) + mParameters.v0();\n    }\n    \n    void MeiCamera::undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const\n    {\n        Eigen::Vector2d p_d;\n\n        if (m_noDistortion)\n        {\n            p_d = p_u;\n        }\n        else\n        {\n            Eigen::Vector2d d_u;\n            distortion(p_u, d_u);\n            p_d = p_u + d_u;\n        }\n\n        p << mParameters.gamma1()*p_d(0) + mParameters.u0(),\n             mParameters.gamma2()*p_d(1) + mParameters.v0();\n    }\n    \n    void MeiCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const\n    {\n        double k1 = mParameters.k1();\n        double k2 = mParameters.k2();\n        double p1 = mParameters.p1();\n        double p2 = mParameters.p2();\n        \n        double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u;\n        \n        mx2_u = p_u(0)*p_u(0);\n        my2_u = p_u(1)*p_u(1);\n        mxy_u = p_u(0)*p_u(1);\n        \n        rho2_u = mx2_u + my2_u;\n        rad_dist_u = k1*rho2_u + k2*rho2_u*rho2_u;\n        \n        d_u(0) = p_u(0)*rad_dist_u + 2.0*p1*mxy_u + p2*(rho2_u + 2.0*mx2_u);\n        d_u(1) = p_u(1)*rad_dist_u + 2.0*p2*mxy_u + p1*(rho2_u + 2.0*my2_u);\n    }\n    \n    void MeiCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u, Eigen::Matrix2d& J) const\n    {\n        double k1 = mParameters.k1();\n        double k2 = mParameters.k2();\n        double p1 = mParameters.p1();\n        double p2 = mParameters.p2();\n        \n        double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u;\n        \n        mx2_u = p_u(0)*p_u(0);\n        my2_u = p_u(1)*p_u(1);\n        mxy_u = p_u(0)*p_u(1);\n        \n        rho2_u = mx2_u + my2_u;\n        rad_dist_u = k1*rho2_u + k2*rho2_u*rho2_u;\n        \n        d_u(0) = p_u(0)*rad_dist_u + 2.0*p1*mxy_u + p2*(rho2_u + 2.0*mx2_u);\n        d_u(1) = p_u(1)*rad_dist_u + 2.0*p2*mxy_u + p1*(rho2_u + 2.0*my2_u);\n\n        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);\n        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);       \n        J(1, 0) = J(0, 1);\n        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);\n    }\n    \n    int MeiCamera::parameterCount() const\n    {\n        return 9;\n    }\n\n    const MeiCamera::Parameters& MeiCamera::getParameters() const\n    {\n        return mParameters;\n    }\n\n    void MeiCamera::setParameters(const MeiCamera::Parameters& parameters)\n    {\n        mParameters = parameters;\n\n        if ((mParameters.k1() == 0.0) && (mParameters.k2() == 0.0) && (mParameters.p1() == 0.0) && (mParameters.p2() == 0.0))\n        {\n            m_noDistortion = true;\n        }\n        else\n        {\n            m_noDistortion = false;\n        }\n\n        m_inv_K11 = 1.0 / mParameters.gamma1();\n        m_inv_K13 = -mParameters.u0() / mParameters.gamma1();\n        m_inv_K22 = 1.0 / mParameters.gamma2();\n        m_inv_K23 = -mParameters.v0() / mParameters.gamma2();\n    }\n    \n    void MeiCamera::readParameters(const std::vector<double>& parameterVec)\n    {\n        if ((int)parameterVec.size() != parameterCount())\n            return;\n\n        Parameters params = getParameters();\n\n        params.xi() = parameterVec.at(0);\n        params.k1() = parameterVec.at(1);\n        params.k2() = parameterVec.at(2);\n        params.p1() = parameterVec.at(3);\n        params.p2() = parameterVec.at(4);\n        params.gamma1() = parameterVec.at(5);\n        params.gamma2() = parameterVec.at(6);\n        params.u0() = parameterVec.at(7);\n        params.v0() = parameterVec.at(8);\n\n        setParameters(params);\n    }\n    \n    void MeiCamera::writeParameters(std::vector<double>& parameterVec) const\n    {\n        parameterVec.resize(parameterCount());\n        parameterVec.at(0) = mParameters.xi();\n        parameterVec.at(1) = mParameters.k1();\n        parameterVec.at(2) = mParameters.k2();\n        parameterVec.at(3) = mParameters.p1();\n        parameterVec.at(4) = mParameters.p2();\n        parameterVec.at(5) = mParameters.gamma1();\n        parameterVec.at(6) = mParameters.gamma2();\n        parameterVec.at(7) = mParameters.u0();\n        parameterVec.at(8) = mParameters.v0();\n    }\n\n    void MeiCamera::writeParametersToYamlFile(const std::string& filename) const\n    {\n        mParameters.writeToYamlFile(filename);\n    }\n\n    std::string MeiCamera::parametersToString() const\n    {\n        std::ostringstream oss;\n        oss << mParameters;\n\n        return oss.str();\n    }\n\n}\n"
  },
  {
    "path": "camera_model/src/PinholeCamera.cpp",
    "content": "/**   This File is part of Camera Model\n *  \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n *    \n *    Since some functions are adapted from GVINS <https://github.com/HKUST-Aerial-Robotics/GVINS>\n *    GVINS is under GPLv3 License.\n */\n\n#include \"PinholeCamera.h\"\n#include <iomanip>\n\nnamespace camera_model\n{\n    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)\n    {}\n    \n    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)\n    {}\n    \n    double& PinholeCamera::Parameters::k1(void) { return m_k1;}\n\n    double& PinholeCamera::Parameters::k2(void) { return m_k2;}\n\n    double& PinholeCamera::Parameters::p1(void) { return m_p1;}\n\n    double& PinholeCamera::Parameters::p2(void) { return m_p2;}\n\n    double& PinholeCamera::Parameters::fx(void) { return m_fx;}\n\n    double& PinholeCamera::Parameters::fy(void) { return m_fy;}\n\n    double& PinholeCamera::Parameters::cx(void) { return m_cx;}\n\n    double& PinholeCamera::Parameters::cy(void) { return m_cy;}\n\n    double PinholeCamera::Parameters::k1(void) const { return m_k1;}\n\n    double PinholeCamera::Parameters::k2(void) const { return m_k2;}\n\n    double PinholeCamera::Parameters::p1(void) const { return m_p1;}\n\n    double PinholeCamera::Parameters::p2(void) const { return m_p2;}\n\n    double PinholeCamera::Parameters::fx(void) const { return m_fx;}\n\n    double PinholeCamera::Parameters::fy(void) const { return m_fy;}\n\n    double PinholeCamera::Parameters::cx(void) const { return m_cx;}\n\n    double PinholeCamera::Parameters::cy(void) const { return m_cy;}\n    \n    bool PinholeCamera::Parameters::readFromYamlFile(const std::string& filename)\n    {\n        cv::FileStorage fs(filename, cv::FileStorage::READ);\n\n        if (!fs.isOpened())\n        {\n            return false;\n        }\n\n        if (!fs[\"model_type\"].isNone())\n        {\n            std::string sModelType;\n            fs[\"model_type\"] >> sModelType;\n\n            if (sModelType.compare(\"PINHOLE\") != 0)\n            {\n                return false;\n            }\n        }\n\n        m_modelType = PINHOLE;\n        fs[\"camera_name\"] >> m_cameraName;\n        m_imageWidth = static_cast<int>(fs[\"image_width\"]);\n        m_imageHeight = static_cast<int>(fs[\"image_height\"]);\n\n        cv::FileNode n = fs[\"distortion_parameters\"];\n        m_k1 = static_cast<double>(n[\"k1\"]);\n        m_k2 = static_cast<double>(n[\"k2\"]);\n        m_p1 = static_cast<double>(n[\"p1\"]);\n        m_p2 = static_cast<double>(n[\"p2\"]);\n\n        n = fs[\"projection_parameters\"];\n        m_fx = static_cast<double>(n[\"fx\"]);\n        m_fy = static_cast<double>(n[\"fy\"]);\n        m_cx = static_cast<double>(n[\"cx\"]);\n        m_cy = static_cast<double>(n[\"cy\"]);\n    \n        return true;\n    }\n    \n    void PinholeCamera::Parameters::writeToYamlFile(const std::string& filename) const\n    {\n        cv::FileStorage fs(filename, cv::FileStorage::WRITE);\n\n        fs << \"model_type\" << \"PINHOLE\";\n        fs << \"camera_name\" << m_cameraName;\n        fs << \"image_width\" << m_imageWidth;\n        fs << \"image_height\" << m_imageHeight;\n\n        // radial distortion: k1, k2\n        // tangential distortion: p1, p2\n        fs << \"distortion_parameters\";\n        fs << \"{\" << \"k1\" << m_k1\n                  << \"k2\" << m_k2\n                  << \"p1\" << m_p1\n                  << \"p2\" << m_p2 << \"}\";\n\n        // projection: fx, fy, cx, cy\n        fs << \"projection_parameters\";\n        fs << \"{\" << \"fx\" << m_fx\n                  << \"fy\" << m_fy\n                  << \"cx\" << m_cx\n                  << \"cy\" << m_cy << \"}\";\n\n        fs.release();\n    }\n    \n    PinholeCamera::Parameters& PinholeCamera::Parameters::operator=(const PinholeCamera::Parameters& other)\n    {\n        if (this != &other)\n        {\n            m_modelType = other.m_modelType;\n            m_cameraName = other.m_cameraName;\n            m_imageWidth = other.m_imageWidth;\n            m_imageHeight = other.m_imageHeight;\n            m_k1 = other.m_k1;\n            m_k2 = other.m_k2;\n            m_p1 = other.m_p1;\n            m_p2 = other.m_p2;\n            m_fx = other.m_fx;\n            m_fy = other.m_fy;\n            m_cx = other.m_cx;\n            m_cy = other.m_cy;\n        }\n        return *this;\n    }\n\n    std::ostream& operator<< (std::ostream& out, const PinholeCamera::Parameters& params)\n    {\n        out << \"Camera Parameters:\" << std::endl;\n        out << \"    model_type \" << \"PINHOLE\" << std::endl;\n        out << \"   camera_name \" << params.m_cameraName << std::endl;\n        out << \"   image_width \" << params.m_imageWidth << std::endl;\n        out << \"  image_height \" << params.m_imageHeight << std::endl;\n\n        // radial distortion: k1, k2\n        // tangential distortion: p1, p2\n        out << \"Distortion Parameters\" << std::endl;\n        out << \"            k1 \" << params.m_k1 << std::endl\n            << \"            k2 \" << params.m_k2 << std::endl\n            << \"            p1 \" << params.m_p1 << std::endl\n            << \"            p2 \" << params.m_p2 << std::endl;\n\n        // projection: fx, fy, cx, cy\n        out << \"Projection Parameters\" << std::endl;\n        out << \"            fx \" << params.m_fx << std::endl\n            << \"            fy \" << params.m_fy << std::endl\n            << \"            cx \" << params.m_cx << std::endl\n            << \"            cy \" << params.m_cy << std::endl;\n\n        return out;\n    }\n    \n    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)\n    {}\n\n    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)\n    {\n        if ((mParameters.k1() == 0.0) && (mParameters.k2() == 0.0) && (mParameters.p1() == 0.0) && (mParameters.p2() == 0.0))\n        {\n            m_noDistortion = true;\n        }\n        else\n        {\n            m_noDistortion = false;\n        }\n\n        m_inv_K11 = 1.0 / mParameters.fx();\n        m_inv_K13 = -mParameters.cx() / mParameters.fx();\n        m_inv_K22 = 1.0 / mParameters.fy();\n        m_inv_K23 = -mParameters.cy() / mParameters.fy();\n    }\n\n    PinholeCamera::PinholeCamera(const PinholeCamera::Parameters& params) : mParameters(params)\n    {\n        if ((mParameters.k1() == 0.0) && (mParameters.k2() == 0.0) && (mParameters.p1() == 0.0) && (mParameters.p2() == 0.0))\n        {\n            m_noDistortion = true;\n        }\n        else\n        {\n            m_noDistortion = false;\n        }\n\n        m_inv_K11 = 1.0 / mParameters.fx();\n        m_inv_K13 = -mParameters.cx() / mParameters.fx();\n        m_inv_K22 = 1.0 / mParameters.fy();\n        m_inv_K23 = -mParameters.cy() / mParameters.fy();\n    }\n    \n    Camera::ModelType PinholeCamera::modelType(void) const\n    {\n        return mParameters.modelType();\n    }\n\n    const std::string& PinholeCamera::cameraName(void) const\n    {\n        return mParameters.cameraName();\n    }\n\n    int PinholeCamera::imageWidth(void) const\n    {\n        return mParameters.imageWidth();\n    }\n\n    int PinholeCamera::imageHeight(void) const\n    {\n        return mParameters.imageHeight();\n    }\n    \n    double PinholeCamera::getNormalPixel() const\n    {\n        return 2.0/(mParameters.fx() + mParameters.fy());\n    }\n    \n    void PinholeCamera::liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const\n    {\n        liftProjective(p, P);\n\n        P.normalize();\n    }\n    \n    void PinholeCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const\n    {\n        double mx_d, my_d,mx2_d, mxy_d, my2_d, mx_u, my_u;\n        double rho2_d, rho4_d, radDist_d, Dx_d, Dy_d, inv_denom_d;\n\n        mx_d = m_inv_K11 * p(0) + m_inv_K13;\n        my_d = m_inv_K22 * p(1) + m_inv_K23;\n\n        if (m_noDistortion)\n        {\n            mx_u = mx_d;\n            my_u = my_d;\n        }\n        else\n        {\n            double k1 = mParameters.k1();\n            double k2 = mParameters.k2();\n            double p1 = mParameters.p1();\n            double p2 = mParameters.p2();\n\n            mx2_d = mx_d*mx_d;\n            my2_d = my_d*my_d;\n            mxy_d = mx_d*my_d;\n            rho2_d = mx2_d+my2_d;\n            rho4_d = rho2_d*rho2_d;\n            radDist_d = k1*rho2_d+k2*rho4_d;\n            Dx_d = mx_d*radDist_d + p2*(rho2_d+2*mx2_d) + 2*p1*mxy_d;\n            Dy_d = my_d*radDist_d + p1*(rho2_d+2*my2_d) + 2*p2*mxy_d;\n            inv_denom_d = 1/(1+4*k1*rho2_d+6*k2*rho4_d+8*p1*my_d+8*p2*mx_d);\n\n            mx_u = mx_d - inv_denom_d*Dx_d;\n            my_u = my_d - inv_denom_d*Dy_d;\n        }\n\n        P << mx_u, my_u, 1.0;\n    }\n    \n    void PinholeCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const\n    {\n        Eigen::Vector2d p_u, p_d;\n\n        p_u << P(0) / P(2), P(1) / P(2);\n\n        if (m_noDistortion)\n        {\n            p_d = p_u;\n        }\n        else\n        {\n            Eigen::Vector2d d_u;\n            distortion(p_u, d_u);\n            p_d = p_u + d_u;\n        }\n\n        p << mParameters.fx() * p_d(0) + mParameters.cx(),\n             mParameters.fy() * p_d(1) + mParameters.cy();\n    }\n    \n    void PinholeCamera::spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,  Eigen::Matrix<double,2,3>& J) const\n    {\n        Eigen::Vector2d p_u, p_d;\n        double norm, inv_denom;\n        double dxdmx, dydmx, dxdmy, dydmy;\n\n        norm = P.norm();\n\n        inv_denom = 1.0 / P(2);\n        p_u << inv_denom * P(0), inv_denom * P(1);\n\n        double dudx = inv_denom;\n        double dvdx = 0.0;\n        double dudy = 0.0;\n        double dvdy = inv_denom;\n        inv_denom = - inv_denom * inv_denom;\n        double dudz = P(0) * inv_denom;\n        double dvdz = P(1) * inv_denom;\n\n        if (m_noDistortion)\n        {\n            p_d = p_u;\n        }\n        else\n        {\n            Eigen::Vector2d d_u;\n            distortion(p_u, d_u);\n            p_d = p_u + d_u;\n        }\n\n        double fx = mParameters.fx();\n        double fy = mParameters.fy();\n\n        // Make the product of the jacobians\n        // and add projection matrix jacobian\n        inv_denom = fx * (dudx * dxdmx + dvdx * dxdmy); // reuse\n        dvdx = fy * (dudx * dydmx + dvdx * dydmy);\n        dudx = inv_denom;\n\n        inv_denom = fx * (dudy * dxdmx + dvdy * dxdmy); // reuse\n        dvdy = fy * (dudy * dydmx + dvdy * dydmy);\n        dudy = inv_denom;\n\n        inv_denom = fx * (dudz * dxdmx + dvdz * dxdmy); // reuse\n        dvdz = fy * (dudz * dydmx + dvdz * dydmy);\n        dudz = inv_denom;\n\n        // Apply generalised projection matrix\n        p << fx * p_d(0) + mParameters.cx(),\n             fy * p_d(1) + mParameters.cy();\n\n        J << dudx, dudy, dudz,\n             dvdx, dvdy, dvdz;\n    }\n\n    void PinholeCamera::undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const\n    {\n        Eigen::Vector2d p_d;\n\n        if (m_noDistortion)\n        {\n            p_d = p_u;\n        }\n        else\n        {\n            // Apply distortion\n            Eigen::Vector2d d_u;\n            distortion(p_u, d_u);\n            p_d = p_u + d_u;\n        }\n\n        // Apply generalised projection matrix\n        p << mParameters.fx() * p_d(0) + mParameters.cx(),\n             mParameters.fy() * p_d(1) + mParameters.cy();\n    }\n    \n    void PinholeCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const\n    {\n        double k1 = mParameters.k1();\n        double k2 = mParameters.k2();\n        double p1 = mParameters.p1();\n        double p2 = mParameters.p2();\n\n        double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u;\n\n        mx2_u = p_u(0) * p_u(0);\n        my2_u = p_u(1) * p_u(1);\n        mxy_u = p_u(0) * p_u(1);\n        rho2_u = mx2_u + my2_u;\n        rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u;\n        d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u),\n               p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u);\n    }\n    \n    void PinholeCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u,  Eigen::Matrix2d& J) const\n    {\n        double k1 = mParameters.k1();\n        double k2 = mParameters.k2();\n        double p1 = mParameters.p1();\n        double p2 = mParameters.p2();\n\n        double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u;\n\n        mx2_u = p_u(0) * p_u(0);\n        my2_u = p_u(1) * p_u(1);\n        mxy_u = p_u(0) * p_u(1);\n        rho2_u = mx2_u + my2_u;\n        rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u;\n        d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u),\n               p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u);\n\n        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);\n        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);\n        double dxdmy = dydmx;\n        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);\n\n        J << dxdmx, dxdmy,\n             dydmx, dydmy;\n    }\n    \n    int PinholeCamera::parameterCount(void) const\n    {\n        return 8;\n    }\n\n    const PinholeCamera::Parameters& PinholeCamera::getParameters(void) const\n    {\n        return mParameters;\n    }\n\n    void PinholeCamera::setParameters(const PinholeCamera::Parameters& parameters)\n    {\n        mParameters = parameters;\n\n        if ((mParameters.k1() == 0.0) && (mParameters.k2() == 0.0) && (mParameters.p1() == 0.0) && (mParameters.p2() == 0.0))\n        {\n            m_noDistortion = true;\n        }\n        else\n        {\n            m_noDistortion = false;\n        }\n\n        m_inv_K11 = 1.0 / mParameters.fx();\n        m_inv_K13 = -mParameters.cx() / mParameters.fx();\n        m_inv_K22 = 1.0 / mParameters.fy();\n        m_inv_K23 = -mParameters.cy() / mParameters.fy();\n    }\n\n    void PinholeCamera::readParameters(const std::vector<double>& parameterVec)\n    {\n        if ((int)parameterVec.size() != parameterCount())\n        {\n            return;\n        }\n\n        Parameters params = getParameters();\n\n        params.k1() = parameterVec.at(0);\n        params.k2() = parameterVec.at(1);\n        params.p1() = parameterVec.at(2);\n        params.p2() = parameterVec.at(3);\n        params.fx() = parameterVec.at(4);\n        params.fy() = parameterVec.at(5);\n        params.cx() = parameterVec.at(6);\n        params.cy() = parameterVec.at(7);\n\n        setParameters(params);\n    }\n\n    void PinholeCamera::writeParameters(std::vector<double>& parameterVec) const\n    {\n        parameterVec.resize(parameterCount());\n        parameterVec.at(0) = mParameters.k1();\n        parameterVec.at(1) = mParameters.k2();\n        parameterVec.at(2) = mParameters.p1();\n        parameterVec.at(3) = mParameters.p2();\n        parameterVec.at(4) = mParameters.fx();\n        parameterVec.at(5) = mParameters.fy();\n        parameterVec.at(6) = mParameters.cx();\n        parameterVec.at(7) = mParameters.cy();\n    }\n\n    void PinholeCamera::writeParametersToYamlFile(const std::string& filename) const\n    {\n        mParameters.writeToYamlFile(filename);\n    }\n\n    std::string PinholeCamera::parametersToString(void) const\n    {\n        std::ostringstream oss;\n        oss << mParameters;\n\n        return oss.str();\n    }\n    \n}\n"
  },
  {
    "path": "camera_model/src/camera.cpp",
    "content": "/**   This File is part of Camera Model\n *  \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n *    \n *    Since some functions are adapted from GVINS <https://github.com/HKUST-Aerial-Robotics/GVINS>\n *    GVINS is under GPLv3 License.\n */\n\n#include \"camera.h\"\n\nnamespace camera_model\n{\n    Camera::Parameters::Parameters(ModelType modelType): m_modelType(modelType), m_imageWidth(0), m_imageHeight(0)\n    {\n        switch (modelType)\n        {\n            case PINHOLE:\n                m_nIntrinsics = 8;\n                break;\n            case MEI:\n            default:\n                m_nIntrinsics = 9;\n        }\n    }\n    \n    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)\n    {\n        switch (modelType)\n        {\n            case PINHOLE:\n                m_nIntrinsics = 8;\n                break;\n            case MEI:\n            default:\n                m_nIntrinsics = 9;\n        }\n    }\n    \n    Camera::ModelType& Camera::Parameters::modelType()\n    {\n        return m_modelType;\n    }\n    \n    std::string& Camera::Parameters::cameraName()\n    {\n        return m_cameraName;\n    }\n    \n    int& Camera::Parameters::imageWidth()\n    {\n        return m_imageWidth;\n    }\n\n    int& Camera::Parameters::imageHeight()\n    {\n        return m_imageHeight;\n    }\n\n    Camera::ModelType Camera::Parameters::modelType() const \n    {\n        return m_modelType;\n    }\n\n    const std::string& Camera::Parameters::cameraName() const\n    {\n        return m_cameraName;\n    }\n\n    int Camera::Parameters::imageWidth() const\n    {\n        return m_imageWidth;\n    }\n\n    int Camera::Parameters::imageHeight() const\n    {\n        return m_imageHeight;\n    }\n\n    int Camera::Parameters::nIntrinsics() const\n    {\n        return m_nIntrinsics;\n    }\n\n    cv::Mat& Camera::mask()\n    {\n        return m_mask;\n    }\n\n    const cv::Mat& Camera::mask() const\n    {\n        return m_mask;\n    }    \n    \n    double Camera::reprojectionDist(const Eigen::Vector3d& P1, const Eigen::Vector3d& P2) const\n    {\n        Eigen::Vector2d p1, p2;\n        \n        spaceToPlane(P1, p1);\n        spaceToPlane(P2, p2);\n        \n        return (p1-p2).norm();\n    }\n    \n    double Camera::reprojectionError(const Eigen::Vector3d& P, \n                                     const Eigen::Quaterniond& camera_q, \n                                     const Eigen::Vector3d& camera_t, \n                                     const Eigen::Vector2d& observed_p) const\n    {\n        Eigen::Vector3d P_cam = camera_q.toRotationMatrix() * P + camera_t;\n\n        Eigen::Vector2d p;\n        spaceToPlane(P_cam, p);\n\n        return (p - observed_p).norm();\n    }\n}\n"
  },
  {
    "path": "camera_model/src/camera_factory.cpp",
    "content": "/**   This File is part of Camera Model\n *  \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n *    \n *    Since some functions are adapted from GVINS <https://github.com/HKUST-Aerial-Robotics/GVINS>\n *    GVINS is under GPLv3 License.\n */\n\n#include \"camera_factory.h\"\n#include \"MeiCamera.h\"\n#include \"PinholeCamera.h\"\n\n#include <boost/algorithm/string.hpp>\n\nnamespace camera_model\n{\n    boost::shared_ptr<CameraFactory> CameraFactory::m_instance;\n    \n    CameraFactory::CameraFactory() {}\n    \n    boost::shared_ptr<CameraFactory> CameraFactory::instance(void)\n    {\n        if (m_instance.get() == 0)\n            m_instance.reset(new CameraFactory);\n        \n        return m_instance;\n    }\n    \n    CameraPtr CameraFactory::generateCamera(Camera::ModelType modelType, const std::string& cameraName, cv::Size imageSize) const\n    {\n        switch (modelType)\n        {\n            case Camera::PINHOLE:\n            {\n                PinholeCameraPtr camera(new PinholeCamera);\n                \n                PinholeCamera::Parameters params = camera->getParameters();\n                params.cameraName() = cameraName;\n                params.imageWidth() = imageSize.width;\n                params.imageHeight() = imageSize.height;\n                \n                camera->setParameters(params);\n                return camera;\n            }\n            case Camera::MEI:\n            default:\n            {\n                MeiCameraPtr camera(new MeiCamera);\n                \n                MeiCamera::Parameters params = camera->getParameters();\n                params.cameraName() = cameraName;\n                params.imageWidth() = imageSize.width;\n                params.imageHeight() = imageSize.height;\n                \n                camera->setParameters(params);\n                return camera;\n            }\n        }\n    }\n    \n    CameraPtr CameraFactory::generateCameraFromYamlFile(const std::string& filename)\n    {\n        cv::FileStorage fs(filename, cv::FileStorage::READ);\n        \n        if (!fs.isOpened())\n            return CameraPtr();\n        \n        Camera::ModelType modelType = Camera::MEI;\n        if (!fs[\"model_type\"].isNone())\n        {\n            std::string sModelType;\n            fs[\"model_type\"] >> sModelType;\n            \n            if (boost::iequals(sModelType, \"MEI\"))\n            {\n                modelType = Camera::MEI;\n            }\n            else if (boost::iequals(sModelType, \"PINHOLE\"))\n            {\n                modelType = Camera::PINHOLE;\n            }\n            else\n            {\n                std::cerr << \"# ERROR: Unknown camera model type: \" << sModelType << std::endl;\n                return CameraPtr();\n            }\n        }\n        \n        switch (modelType)\n        {\n            case Camera::PINHOLE:\n            {\n                PinholeCameraPtr camera(new PinholeCamera);\n                \n                PinholeCamera::Parameters params = camera->getParameters();\n                params.readFromYamlFile(filename);\n                \n                camera->setParameters(params);\n                return camera;\n            }\n            case Camera::MEI:\n            default:\n            {\n                MeiCameraPtr camera(new MeiCamera);\n                \n                MeiCamera::Parameters params = camera->getParameters();\n                params.readFromYamlFile(filename);\n                \n                camera->setParameters(params);\n                return camera;\n            }\n        }\n        \n        return CameraPtr();\n    }\n    \n}\n"
  },
  {
    "path": "camera_model/test/CMakeLists.txt",
    "content": "# find_package(GTest REQUIRED)\n# include_directories(${GTEST_INCLUDE_DIRS})\n\n# add each test source file and executable items\n# add_executable(test_common test_common.cpp)\n# target_link_libraries(test_common ${GTEST_BOTH_LIBRARIES} pthread)\n# gtest_discover_tests(test_common)\n"
  },
  {
    "path": "config/fw_zed2i_f9p/ingvio_mono.yaml",
    "content": "%YAML:1.0\n# config for fw_zed2i_f9p\n\nimu_topic: \"/imu0\"\nfeature_topic: \"/mono_tracker/mono_feature\"\n\ncam_nums: 1\ncam_left_file_path: \"/home/lcw/VIO/ws_ingvio/src/config/fw_zed2i_f9p/mono_config.yaml\"\n\nmax_sliding_window_poses: 25\nis_key_frame: 1\nmax_landmark_features: 0\n\nenable_gnss: 1\n\nnoise_gyro: 0.005\nnoise_accel: 0.09\nnoise_bias_gyro: 0.0005\nnoise_bias_accel: 0.009\nnoise_rcv_clockbias: 2.0\nnoise_rcv_clockbias_randomwalk: 0.2\n\ninit_cov_rot: 0.0\ninit_cov_pos: 0.0\ninit_cov_vel: 0.25\ninit_cov_bg: 0.01\ninit_cov_ba: 0.01\ninit_cov_ext_rot: 1.8e-02\ninit_cov_ext_pos: 2e-03\ninit_cov_rcv_clockbias: 2.0\ninit_cov_rcv_clockbias_randomwalk: 1.0\ninit_cov_yof: 0.015\n\ngravity_norm: 9.8\nmax_imu_buffer_size: 10000\ninit_imu_buffer_sp: 100\n\ntrans_thres: 0.25\nhuber_epsilon: 0.01\nconv_precision: 5e-08\ninit_damping: 1e-03\nouter_loop_max_iter: 10\ninner_loop_max_iter: 10\nmax_depth: 80.0\nmin_depth: 0.2\nmax_baseline_ratio: 80.0\n\nchi2_max_dof: 150\nchi2_thres: 0.95\nvisual_noise: 0.08\nframe_select_interval: 23\n\ngnss_ephem_topic: \"/ublox_driver/ephem\"\ngnss_glo_ephem_topic: \"/ublox_driver/glo_ephem\"\ngnss_meas_topic: \"/ublox_driver/range_meas\"\ngnss_iono_params_topic: \"/ublox_driver/iono_params\"\nrtk_gt_topic: \"/ublox_driver/receiver_lla\"\n\ngnss_elevation_thres: 20.0\ngnss_psr_std_thres: 8.0\ngnss_dopp_std_thres: 8.0\ngnss_track_num_thres: 20\n\nuse_fix_time_offset: 1\ngnss_local_offset: -970047.18\ngnss_chi2_test: 0\ngnss_strong_reject: 0\n\ngv_align_batch_size: 25\ngv_align_max_iter: 10\ngv_align_conv_epsilon: 1e-05       \ngv_align_vel_thres: 0.4\n\npsr_noise_amp: 1.0\ndopp_noise_amp: 1.0\nis_adjust_yof: 0\n"
  },
  {
    "path": "config/fw_zed2i_f9p/ingvio_stereo.yaml",
    "content": "%YAML:1.0\n# config for fw_zed2i_f9p\n\nimu_topic: \"/imu0\"\nfeature_topic: \"/stereo_tracker/stereo_feature\"\n\ncam_nums: 2\ncam_left_file_path: \"/home/lcw/VIO/ws_ingvio/src/config/fw_zed2i_f9p/stereo_left_config.yaml\"\ncam_right_file_path: \"/home/lcw/VIO/ws_ingvio/src/config/fw_zed2i_f9p/stereo_right_config.yaml\"\n\nmax_sliding_window_poses: 21\nis_key_frame: 1\nmax_landmark_features: 0\n\nenable_gnss: 1\n\nnoise_gyro: 0.005\nnoise_accel: 0.09\nnoise_bias_gyro: 0.0005\nnoise_bias_accel: 0.009\nnoise_rcv_clockbias: 2.0\nnoise_rcv_clockbias_randomwalk: 0.2\n\ninit_cov_rot: 0.0\ninit_cov_pos: 0.0\ninit_cov_vel: 0.25\ninit_cov_bg: 0.01\ninit_cov_ba: 0.01\ninit_cov_ext_rot: 1.8e-02\ninit_cov_ext_pos: 2e-03\ninit_cov_rcv_clockbias: 2.0\ninit_cov_rcv_clockbias_randomwalk: 1.0\ninit_cov_yof: 0.015\n\ngravity_norm: 9.8\nmax_imu_buffer_size: 10000\ninit_imu_buffer_sp: 100\n\ntrans_thres: 0.25\nhuber_epsilon: 0.01\nconv_precision: 5e-08\ninit_damping: 1e-03\nouter_loop_max_iter: 10\ninner_loop_max_iter: 10\nmax_depth: 80.0\nmin_depth: 0.2\nmax_baseline_ratio: 80.0\n\nchi2_max_dof: 150\nchi2_thres: 0.95\nvisual_noise: 0.08\nframe_select_interval: 18\n\ngnss_ephem_topic: \"/ublox_driver/ephem\"\ngnss_glo_ephem_topic: \"/ublox_driver/glo_ephem\"\ngnss_meas_topic: \"/ublox_driver/range_meas\"\ngnss_iono_params_topic: \"/ublox_driver/iono_params\"\nrtk_gt_topic: \"/ublox_driver/receiver_lla\"\n\ngnss_elevation_thres: 20.0\ngnss_psr_std_thres: 8.0\ngnss_dopp_std_thres: 8.0\ngnss_track_num_thres: 20\n\nuse_fix_time_offset: 1\ngnss_local_offset:  -970047.18\ngnss_chi2_test: 0\ngnss_strong_reject: 0\n\ngv_align_batch_size: 25\ngv_align_max_iter: 10\ngv_align_conv_epsilon: 1e-05       \ngv_align_vel_thres: 0.4\n\npsr_noise_amp: 1.0\ndopp_noise_amp: 1.0\nis_adjust_yof: 0\n\n"
  },
  {
    "path": "config/fw_zed2i_f9p/mono_config.yaml",
    "content": "%YAML:1.0\n# config for fw_zed2i_f9p\n\n# common parameters\nimu_topic: \"/imu0\"\nimage_topic: \"/cam0/image_raw\"\n\n# intrinsic\nmodel_type: PINHOLE\ncamera_name: cam0\nimage_width: 1280\nimage_height: 720\ndistortion_parameters:\n   k1: -0.013323572637177162\n   k2: 0.012297484726727028\n   p1: -0.0008173313551158254\n   p2: -0.0036129297585792528\nprojection_parameters:\n   fx: 528.0821562999166\n   fy: 528.1818555071027\n   cx: 646.6309796410819\n   cy: 351.87255392593374\n   \n# extrinsic\nextrinsicRotation: !!opencv-matrix        # R^{imu}_{cam}\n   rows: 3\n   cols: 3\n   dt: d\n   data: [0.01301095634388352, -0.005070623738001712, 0.9999024971415602, \n          -0.9999097972900367, 0.003267771869320563, 0.01302762259241452, \n         -0.003333511424589375, -0.9999818050554322, -0.005027649516865795]\nextrinsicTranslation: !!opencv-matrix     # t^{imu}_{cam}\n   rows: 3\n   cols: 1\n   dt: d\n   data: [0.03671615355111148, 0.02169026717979497, -0.0000578135929000942]\n   \n\n# feature tracker parameters\nmax_cnt: 100        # max feature number in feature tracking\nmin_dist: 35        # min distance between two features \nfreq: 0             # freq (Hz) of publishing tracking result. At least 10Hz for good\n                    # estimation. If set 0, the frequence will be the same as raw image \nF_threshold: 1.0    # ransac threshold (pixel)\nshow_track: 1       # publish tracking image as topic\nshow_timer: 0       # show time counting\ntimer_warning_thres: 50.0 # in (ms)\nequalize: 1         # if image is too dark or light, \n                    # turn on equalize to find enough features\n"
  },
  {
    "path": "config/fw_zed2i_f9p/stereo_left_config.yaml",
    "content": "%YAML:1.0\n# config for fw_zed2i_f9p\n\n#topic parameters\nimu_topic: \"/imu0\"\ncam_left_topic: \"/cam0/image_raw\"\ncam_right_topic: \"/cam1/image_raw\"\n\n# intrinsic\nmodel_type: PINHOLE\ncamera_name: cam0\nimage_width: 1280\nimage_height: 720\ndistortion_parameters:\n   k1: -0.013323572637177162\n   k2: 0.012297484726727028\n   p1: -0.0008173313551158254\n   p2: -0.0036129297585792528\nprojection_parameters:\n   fx: 528.0821562999166\n   fy: 528.1818555071027\n   cx: 646.6309796410819\n   cy: 351.87255392593374\n   \n# extrinsic\nextrinsicRotation: !!opencv-matrix        # R^{imu}_{cam}\n   rows: 3\n   cols: 3\n   dt: d\n   data: [0.01301095634388352, -0.005070623738001712, 0.9999024971415602, \n          -0.9999097972900367, 0.003267771869320563, 0.01302762259241452, \n         -0.003333511424589375, -0.9999818050554322, -0.005027649516865795]\nextrinsicTranslation: !!opencv-matrix     # t^{imu}_{cam}\n   rows: 3\n   cols: 1\n   dt: d\n   data: [0.03671615355111148, 0.02169026717979497, -0.0000578135929000942]\n\n# tracker config\nmax_cnt: 60            # max feature number in feature tracking\nmin_dist: 45              # min distance between two features \nfreq: 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 \nshow_track: 1          # publish tracking image as topic\nshow_timer: 0       # show time counting\ntimer_warning_thres: 50.0 # in (ms)\nequalize: 1                # if image is too dark or light, turn on equalize to find enough features\nwindow_size: 20\nepipolar_thres: 15.0\nF_threshold: 1.0\nprint_track_info: 0\n"
  },
  {
    "path": "config/fw_zed2i_f9p/stereo_right_config.yaml",
    "content": "%YAML:1.0\n# config for fw_zed2i_f9p\n\n# intrinsic\nmodel_type: PINHOLE\ncamera_name: cam1\nimage_width: 1280\nimage_height: 720\ndistortion_parameters:\n   k1: -0.013705400292675644\n   k2: 0.01351956571839721\n   p1: -0.000806913038206586\n   p2: -0.0028638404748635034\nprojection_parameters:\n   fx: 527.4644311562542\n   fy: 527.9383388808839\n   cx: 647.6983568199339\n   cy: 351.1977685749355\n\n# extrinsic\nextrinsicRotation: !!opencv-matrix        # R^{imu}_{cam}\n   rows: 3\n   cols: 3\n   dt: d\n   data: [ 0.01129080808837021, -0.004471320310557636, 0.9999262597548844,\n          -0.9999301632412926,   0.00344058703135805, 0.01130623726631103,\n          -0.003490887129951881,-0.9999840847011319, -0.004432161040157589]\nextrinsicTranslation: !!opencv-matrix     # t^{imu}_{cam}\n   rows: 3\n   cols: 1\n   dt: d\n   data: [0.03767186874423049, -0.0980688527108189, -0.0002632245838794373]\n   \n\n"
  },
  {
    "path": "config/sportsfield/ingvio_mono.yaml",
    "content": "%YAML:1.0\n# config for sportsfield\n\nimu_topic: \"/imu0\"\nfeature_topic: \"/mono_tracker/mono_feature\"\n\ncam_nums: 1\ncam_left_file_path: \"/home/lcw/VIO/ws_ingvio/src/config/sportsfield/mono_config.yaml\"\n\nmax_sliding_window_poses: 35\nis_key_frame: 1\nmax_landmark_features: 0\n\nenable_gnss: 1\n\nnoise_gyro: 0.004\nnoise_accel: 0.08\nnoise_bias_gyro: 0.0002\nnoise_bias_accel: 0.008\nnoise_rcv_clockbias: 2.0\nnoise_rcv_clockbias_randomwalk: 0.2\n\ninit_cov_rot: 0.0\ninit_cov_pos: 0.0\ninit_cov_vel: 0.25\ninit_cov_bg: 0.01\ninit_cov_ba: 0.01\ninit_cov_ext_rot: 1.8e-02\ninit_cov_ext_pos: 2e-03\ninit_cov_rcv_clockbias: 2.0\ninit_cov_rcv_clockbias_randomwalk: 1.0\ninit_cov_yof: 0.015\n\ngravity_norm: 9.8\nmax_imu_buffer_size: 3000\ninit_imu_buffer_sp: 300\n\ntrans_thres: 0.25\nhuber_epsilon: 0.01\nconv_precision: 5e-08\ninit_damping: 1e-03\nouter_loop_max_iter: 10\ninner_loop_max_iter: 10\nmax_depth: 60.0\nmin_depth: 0.2\nmax_baseline_ratio: 80.0\n\nchi2_max_dof: 150\nchi2_thres: 0.95\nvisual_noise: 0.18\nframe_select_interval: 28\n\ngnss_ephem_topic: \"/ublox_driver/ephem\"\ngnss_glo_ephem_topic: \"/ublox_driver/glo_ephem\"\ngnss_meas_topic: \"/ublox_driver/range_meas\"\ngnss_iono_params_topic: \"/ublox_driver/iono_params\"\nrtk_gt_topic: \"/ublox_driver/receiver_lla\"\n\ngnss_elevation_thres: 20.0\ngnss_psr_std_thres: 8.0\ngnss_dopp_std_thres: 8.0\ngnss_track_num_thres: 20\n\nuse_fix_time_offset: 1\ngnss_local_offset: -18.0\ngnss_chi2_test: 0\ngnss_strong_reject: 1\n\ngv_align_batch_size: 45\ngv_align_max_iter: 25\ngv_align_conv_epsilon: 1e-05       \ngv_align_vel_thres: 0.4\n\npsr_noise_amp: 1.0\ndopp_noise_amp: 1.0\nis_adjust_yof: 0\n"
  },
  {
    "path": "config/sportsfield/ingvio_stereo.yaml",
    "content": "%YAML:1.0\n# config for sportsfield\n\nimu_topic: \"/imu0\"\nfeature_topic: \"/stereo_tracker/stereo_feature\"\n\ncam_nums: 2\ncam_left_file_path: \"/home/lcw/VIO/ws_ingvio/src/config/sportsfield/stereo_left_config.yaml\"\ncam_right_file_path: \"/home/lcw/VIO/ws_ingvio/src/config/sportsfield/stereo_right_config.yaml\"\n\nmax_sliding_window_poses: 27\nis_key_frame: 1\nmax_landmark_features: 0\n\nenable_gnss: 1\n\nnoise_gyro: 0.004\nnoise_accel: 0.08\nnoise_bias_gyro: 0.0002\nnoise_bias_accel: 0.008\nnoise_rcv_clockbias: 2.0\nnoise_rcv_clockbias_randomwalk: 0.2\n\ninit_cov_rot: 0.0\ninit_cov_pos: 0.0\ninit_cov_vel: 0.25\ninit_cov_bg: 0.01\ninit_cov_ba: 0.01\ninit_cov_ext_rot: 1.8e-02\ninit_cov_ext_pos: 2e-03\ninit_cov_rcv_clockbias: 2.0\ninit_cov_rcv_clockbias_randomwalk: 1.0\ninit_cov_yof: 0.015\n\ngravity_norm: 9.8\nmax_imu_buffer_size: 3000\ninit_imu_buffer_sp: 300\n\ntrans_thres: 0.25\nhuber_epsilon: 0.01\nconv_precision: 5e-07\ninit_damping: 1e-03\nouter_loop_max_iter: 10\ninner_loop_max_iter: 10\nmax_depth: 40.0\nmin_depth: 0.2\nmax_baseline_ratio: 80.0\n\nchi2_max_dof: 150\nchi2_thres: 0.95\nvisual_noise: 0.18\nframe_select_interval: 18\n\ngnss_ephem_topic: \"/ublox_driver/ephem\"\ngnss_glo_ephem_topic: \"/ublox_driver/glo_ephem\"\ngnss_meas_topic: \"/ublox_driver/range_meas\"\ngnss_iono_params_topic: \"/ublox_driver/iono_params\"\nrtk_gt_topic: \"/ublox_driver/receiver_lla\"\n\ngnss_elevation_thres: 20.0\ngnss_psr_std_thres: 8.0\ngnss_dopp_std_thres: 8.0\ngnss_track_num_thres: 20\n\nuse_fix_time_offset: 1\ngnss_local_offset: -18.0\ngnss_chi2_test: 0\ngnss_strong_reject: 1\n\ngv_align_batch_size: 25\ngv_align_max_iter: 10\ngv_align_conv_epsilon: 1e-05       \ngv_align_vel_thres: 0.4\n\npsr_noise_amp: 1.0\ndopp_noise_amp: 1.0\nis_adjust_yof: 0\n"
  },
  {
    "path": "config/sportsfield/mono_config.yaml",
    "content": "%YAML:1.0\n# config for sportsfield\n\n# common parameters\nimu_topic: \"/imu0\"\nimage_topic: \"/cam1/image_raw\"\n\n# camera calibration \nmodel_type: MEI\ncamera_name: camera\nimage_width: 752\nimage_height: 480\nmirror_parameters:\n   xi: 1.8476540167437447\ndistortion_parameters:\n   k1: -0.06597811223735722\n   k2: 0.8559479340704287\n   p1: -0.0006445829733139821\n   p2: 0.0015137487236065916\nprojection_parameters:\n   gamma1: 1338.1845333957547\n   gamma2: 1340.1190112672946\n   u0: 378.7909740462579\n   v0: 217.69105287172025\n\n# extrinsic\nextrinsicRotation: !!opencv-matrix        # R^{imu}_{cam}\n   rows: 3\n   cols: 3\n   dt: d\n   data: [ 0.9999014076382304, -0.0133731297219721, 0.0042818692791948,\n           0.0133731003056063, 0.9999105754655292, 0.0000355022536769,\n          -0.0042819611512717, 0.0000217631139403, 0.9999908321255077 ]\nextrinsicTranslation: !!opencv-matrix     # t^{imu}_{cam}\n   rows: 3\n   cols: 1\n   dt: d\n   data: [0.0341738532732442, -0.0032623030537933, -0.0017782029037505]\n   \n# feature tracker parameters\nmax_cnt: 150        # max feature number in feature tracking\nmin_dist: 30        # min distance between two features \nfreq: 0             # freq (Hz) of publishing tracking result. At least 10Hz for good\n                    # estimation. If set 0, the frequence will be the same as raw image \nF_threshold: 1.0    # ransac threshold (pixel)\nshow_track: 1       # publish tracking image as topic\nshow_timer: 0       # show time counting\ntimer_warning_thres: 50.0 # in (ms)\nequalize: 1         # if image is too dark or light, \n                    # turn on equalize to find enough features\n"
  },
  {
    "path": "config/sportsfield/stereo_left_config.yaml",
    "content": "%YAML:1.0\n# config for sportsfield\n\n#topic parameters\nimu_topic: \"/imu0\"\ncam_left_topic: \"/cam1/image_raw\"\ncam_right_topic: \"/cam0/image_raw\"\n\n# intrinsic\nmodel_type: MEI\ncamera_name: cam1\nimage_width: 752\nimage_height: 480\nmirror_parameters:\n   xi: 1.8476540167437447\ndistortion_parameters:\n   k1: -0.06597811223735722\n   k2: 0.8559479340704287\n   p1: -0.0006445829733139821\n   p2: 0.0015137487236065916\nprojection_parameters:\n   gamma1: 1338.1845333957547\n   gamma2: 1340.1190112672946\n   u0: 378.7909740462579\n   v0: 217.69105287172025\n   \n# extrinsic\nextrinsicRotation: !!opencv-matrix        # R^{imu}_{cam}\n   rows: 3\n   cols: 3\n   dt: d\n   data: [0.9999890386957373, -0.0043227774403168, 0.0017989117755288, \n          0.0043276579084841, 0.9999869417854389, -0.0027180205355500, \n         -0.0017871388870994, 0.0027257758172719, 0.9999946881262878]\nextrinsicTranslation: !!opencv-matrix     # t^{imu}_{cam}\n   rows: 3\n   cols: 1\n   dt: d\n   data: [-0.0759472920952561, -0.0039320527565750, -0.0016395029500217]\n\n# tracker config\nmax_cnt: 110            # max feature number in feature tracking\nmin_dist: 30              # min distance between two features \nfreq: 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 \nshow_track: 1          # publish tracking image as topic\nshow_timer: 0       # show time counting\ntimer_warning_thres: 50.0 # in (ms)\nequalize: 1                # if image is too dark or light, turn on equalize to find enough features\nwindow_size: 20\nepipolar_thres: 25.0\nF_threshold: 2.0\nprint_track_info: 0\n"
  },
  {
    "path": "config/sportsfield/stereo_right_config.yaml",
    "content": "%YAML:1.0\n# config for sportsfield\n\n# intrinsic\nmodel_type: MEI\ncamera_name: cam0\nimage_width: 752\nimage_height: 480\nmirror_parameters:\n   xi: 1.8482000080983196\ndistortion_parameters:\n   k1: -0.05322412328104923\n   k2: 0.7676625579468673\n   p1: -0.00019942420435829978\n   p2: -0.0006936085281739436\nprojection_parameters:\n   gamma1: 1330.3922281659666\n   gamma2: 1332.49270401596\n   u0: 377.39301029010045\n   v0: 233.04955276185672\n\n# extrinsic\nextrinsicRotation: !!opencv-matrix        # R^{imu}_{cam}\n   rows: 3\n   cols: 3\n   dt: d\n   data: [ 0.9999014076382304, -0.0133731297219721, 0.0042818692791948,\n           0.0133731003056063, 0.9999105754655292, 0.0000355022536769,\n          -0.0042819611512717, 0.0000217631139403, 0.9999908321255077 ]\nextrinsicTranslation: !!opencv-matrix     # t^{imu}_{cam}\n   rows: 3\n   cols: 1\n   dt: d\n   data: [0.0341738532732442, -0.0032623030537933, -0.0017782029037505]\n   \n\n"
  },
  {
    "path": "feature_tracker/CMakeLists.txt",
    "content": "cmake_minimum_required(VERSION 2.8.3)\nproject(feature_tracker)\n\nset(CMAKE_BUILD_TYPE \"Release\")\nset(CMAKE_CXX_FLAGS \"-std=c++11\")\n\nfind_package(catkin REQUIRED COMPONENTS\n    roscpp\n    std_msgs\n    message_filters\n    sensor_msgs\n    cv_bridge\n    camera_model\n    message_generation\n)\n\nfind_package(Boost REQUIRED COMPONENTS filesystem program_options system)\nfind_package(OpenCV REQUIRED)\n\n\nadd_message_files(\n   FILES\n   MonoMeas.msg\n   MonoFrame.msg\n   StereoMeas.msg\n   StereoFrame.msg\n)\n\ngenerate_messages(\n   DEPENDENCIES\n   std_msgs\n   geometry_msgs\n)\n\n\ncatkin_package()\n\ninclude_directories(\n    ${PROJECT_SOURCE_DIR}/include\n    \"/usr/include/eigen3\"\n    ${catkin_INCLUDE_DIRS}\n    ${OpenCV_INCLUDE_DIRS}\n    ${Boost_INCLUDE_DIRS}\n)\n\nadd_executable(mono_tracker\n    src/mono_tracker.cpp\n    src/mono_tracker_node.cpp\n    src/mono_parameters.cpp\n)\n\nadd_executable(stereo_tracker\n    src/stereo_tracker.cpp\n    src/stereo_tracker_node.cpp\n    src/stereo_parameters.cpp\n)\n\ntarget_link_libraries(mono_tracker ${catkin_LIBRARIES} ${OpenCV_LIBS})\ntarget_link_libraries(stereo_tracker ${catkin_LIBRARIES} ${OpenCV_LIBS})\n"
  },
  {
    "path": "feature_tracker/LICENSE",
    "content": "                    GNU GENERAL PUBLIC LICENSE\n                       Version 3, 29 June 2007\n\n Copyright (C) 2007 Free Software Foundation, Inc. <https://fsf.org/>\n Everyone is permitted to copy and distribute verbatim copies\n of this license document, but changing it is not allowed.\n\n                            Preamble\n\n  The GNU General Public License is a free, copyleft license for\nsoftware and other kinds of works.\n\n  The licenses for most software and other practical works are designed\nto take away your freedom to share and change the works.  By contrast,\nthe GNU General Public License is intended to guarantee your freedom to\nshare and change all versions of a program--to make sure it remains free\nsoftware for all its users.  We, the Free Software Foundation, use the\nGNU General Public License for most of our software; it applies also to\nany other work released this way by its authors.  You can apply it to\nyour programs, too.\n\n  When we speak of free software, we are referring to freedom, not\nprice.  Our General Public Licenses are designed to make sure that you\nhave the freedom to distribute copies of free software (and charge for\nthem if you wish), that you receive source code or can get it if you\nwant it, that you can change the software or use pieces of it in new\nfree programs, and that you know you can do these things.\n\n  To protect your rights, we need to prevent others from denying you\nthese rights or asking you to surrender the rights.  Therefore, you have\ncertain responsibilities if you distribute copies of the software, or if\nyou modify it: responsibilities to respect the freedom of others.\n\n  For example, if you distribute copies of such a program, whether\ngratis or for a fee, you must pass on to the recipients the same\nfreedoms that you received.  You must make sure that they, too, receive\nor can get the source code.  And you must show them these terms so they\nknow their rights.\n\n  Developers that use the GNU GPL protect your rights with two steps:\n(1) assert copyright on the software, and (2) offer you this License\ngiving you legal permission to copy, distribute and/or modify it.\n\n  For the developers' and authors' protection, the GPL clearly explains\nthat there is no warranty for this free software.  For both users' and\nauthors' sake, the GPL requires that modified versions be marked as\nchanged, so that their problems will not be attributed erroneously to\nauthors of previous versions.\n\n  Some devices are designed to deny users access to install or run\nmodified versions of the software inside them, although the manufacturer\ncan do so.  This is fundamentally incompatible with the aim of\nprotecting users' freedom to change the software.  The systematic\npattern of such abuse occurs in the area of products for individuals to\nuse, which is precisely where it is most unacceptable.  Therefore, we\nhave designed this version of the GPL to prohibit the practice for those\nproducts.  If such problems arise substantially in other domains, we\nstand ready to extend this provision to those domains in future versions\nof the GPL, as needed to protect the freedom of users.\n\n  Finally, every program is threatened constantly by software patents.\nStates should not allow patents to restrict development and use of\nsoftware on general-purpose computers, but in those that do, we wish to\navoid the special danger that patents applied to a free program could\nmake it effectively proprietary.  To prevent this, the GPL assures that\npatents cannot be used to render the program non-free.\n\n  The precise terms and conditions for copying, distribution and\nmodification follow.\n\n                       TERMS AND CONDITIONS\n\n  0. Definitions.\n\n  \"This License\" refers to version 3 of the GNU General Public License.\n\n  \"Copyright\" also means copyright-like laws that apply to other kinds of\nworks, such as semiconductor masks.\n\n  \"The Program\" refers to any copyrightable work licensed under this\nLicense.  Each licensee is addressed as \"you\".  \"Licensees\" and\n\"recipients\" may be individuals or organizations.\n\n  To \"modify\" a work means to copy from or adapt all or part of the work\nin a fashion requiring copyright permission, other than the making of an\nexact copy.  The resulting work is called a \"modified version\" of the\nearlier work or a work \"based on\" the earlier work.\n\n  A \"covered work\" means either the unmodified Program or a work based\non the Program.\n\n  To \"propagate\" a work means to do anything with it that, without\npermission, would make you directly or secondarily liable for\ninfringement under applicable copyright law, except executing it on a\ncomputer or modifying a private copy.  Propagation includes copying,\ndistribution (with or without modification), making available to the\npublic, and in some countries other activities as well.\n\n  To \"convey\" a work means any kind of propagation that enables other\nparties to make or receive copies.  Mere interaction with a user through\na computer network, with no transfer of a copy, is not conveying.\n\n  An interactive user interface displays \"Appropriate Legal Notices\"\nto the extent that it includes a convenient and prominently visible\nfeature that (1) displays an appropriate copyright notice, and (2)\ntells the user that there is no warranty for the work (except to the\nextent that warranties are provided), that licensees may convey the\nwork under this License, and how to view a copy of this License.  If\nthe interface presents a list of user commands or options, such as a\nmenu, a prominent item in the list meets this criterion.\n\n  1. Source Code.\n\n  The \"source code\" for a work means the preferred form of the work\nfor making modifications to it.  \"Object code\" means any non-source\nform of a work.\n\n  A \"Standard Interface\" means an interface that either is an official\nstandard defined by a recognized standards body, or, in the case of\ninterfaces specified for a particular programming language, one that\nis widely used among developers working in that language.\n\n  The \"System Libraries\" of an executable work include anything, other\nthan the work as a whole, that (a) is included in the normal form of\npackaging a Major Component, but which is not part of that Major\nComponent, and (b) serves only to enable use of the work with that\nMajor Component, or to implement a Standard Interface for which an\nimplementation is available to the public in source code form.  A\n\"Major Component\", in this context, means a major essential component\n(kernel, window system, and so on) of the specific operating system\n(if any) on which the executable work runs, or a compiler used to\nproduce the work, or an object code interpreter used to run it.\n\n  The \"Corresponding Source\" for a work in object code form means all\nthe source code needed to generate, install, and (for an executable\nwork) run the object code and to modify the work, including scripts to\ncontrol those activities.  However, it does not include the work's\nSystem Libraries, or general-purpose tools or generally available free\nprograms which are used unmodified in performing those activities but\nwhich are not part of the work.  For example, Corresponding Source\nincludes interface definition files associated with source files for\nthe work, and the source code for shared libraries and dynamically\nlinked subprograms that the work is specifically designed to require,\nsuch as by intimate data communication or control flow between those\nsubprograms and other parts of the work.\n\n  The Corresponding Source need not include anything that users\ncan regenerate automatically from other parts of the Corresponding\nSource.\n\n  The Corresponding Source for a work in source code form is that\nsame work.\n\n  2. Basic Permissions.\n\n  All rights granted under this License are granted for the term of\ncopyright on the Program, and are irrevocable provided the stated\nconditions are met.  This License explicitly affirms your unlimited\npermission to run the unmodified Program.  The output from running a\ncovered work is covered by this License only if the output, given its\ncontent, constitutes a covered work.  This License acknowledges your\nrights of fair use or other equivalent, as provided by copyright law.\n\n  You may make, run and propagate covered works that you do not\nconvey, without conditions so long as your license otherwise remains\nin force.  You may convey covered works to others for the sole purpose\nof having them make modifications exclusively for you, or provide you\nwith facilities for running those works, provided that you comply with\nthe terms of this License in conveying all material for which you do\nnot control copyright.  Those thus making or running the covered works\nfor you must do so exclusively on your behalf, under your direction\nand control, on terms that prohibit them from making any copies of\nyour copyrighted material outside their relationship with you.\n\n  Conveying under any other circumstances is permitted solely under\nthe conditions stated below.  Sublicensing is not allowed; section 10\nmakes it unnecessary.\n\n  3. Protecting Users' Legal Rights From Anti-Circumvention Law.\n\n  No covered work shall be deemed part of an effective technological\nmeasure under any applicable law fulfilling obligations under article\n11 of the WIPO copyright treaty adopted on 20 December 1996, or\nsimilar laws prohibiting or restricting circumvention of such\nmeasures.\n\n  When you convey a covered work, you waive any legal power to forbid\ncircumvention of technological measures to the extent such circumvention\nis effected by exercising rights under this License with respect to\nthe covered work, and you disclaim any intention to limit operation or\nmodification of the work as a means of enforcing, against the work's\nusers, your or third parties' legal rights to forbid circumvention of\ntechnological measures.\n\n  4. Conveying Verbatim Copies.\n\n  You may convey verbatim copies of the Program's source code as you\nreceive it, in any medium, provided that you conspicuously and\nappropriately publish on each copy an appropriate copyright notice;\nkeep intact all notices stating that this License and any\nnon-permissive terms added in accord with section 7 apply to the code;\nkeep intact all notices of the absence of any warranty; and give all\nrecipients a copy of this License along with the Program.\n\n  You may charge any price or no price for each copy that you convey,\nand you may offer support or warranty protection for a fee.\n\n  5. Conveying Modified Source Versions.\n\n  You may convey a work based on the Program, or the modifications to\nproduce it from the Program, in the form of source code under the\nterms of section 4, provided that you also meet all of these conditions:\n\n    a) The work must carry prominent notices stating that you modified\n    it, and giving a relevant date.\n\n    b) The work must carry prominent notices stating that it is\n    released under this License and any conditions added under section\n    7.  This requirement modifies the requirement in section 4 to\n    \"keep intact all notices\".\n\n    c) You must license the entire work, as a whole, under this\n    License to anyone who comes into possession of a copy.  This\n    License will therefore apply, along with any applicable section 7\n    additional terms, to the whole of the work, and all its parts,\n    regardless of how they are packaged.  This License gives no\n    permission to license the work in any other way, but it does not\n    invalidate such permission if you have separately received it.\n\n    d) If the work has interactive user interfaces, each must display\n    Appropriate Legal Notices; however, if the Program has interactive\n    interfaces that do not display Appropriate Legal Notices, your\n    work need not make them do so.\n\n  A compilation of a covered work with other separate and independent\nworks, which are not by their nature extensions of the covered work,\nand which are not combined with it such as to form a larger program,\nin or on a volume of a storage or distribution medium, is called an\n\"aggregate\" if the compilation and its resulting copyright are not\nused to limit the access or legal rights of the compilation's users\nbeyond what the individual works permit.  Inclusion of a covered work\nin an aggregate does not cause this License to apply to the other\nparts of the aggregate.\n\n  6. Conveying Non-Source Forms.\n\n  You may convey a covered work in object code form under the terms\nof sections 4 and 5, provided that you also convey the\nmachine-readable Corresponding Source under the terms of this License,\nin one of these ways:\n\n    a) Convey the object code in, or embodied in, a physical product\n    (including a physical distribution medium), accompanied by the\n    Corresponding Source fixed on a durable physical medium\n    customarily used for software interchange.\n\n    b) Convey the object code in, or embodied in, a physical product\n    (including a physical distribution medium), accompanied by a\n    written offer, valid for at least three years and valid for as\n    long as you offer spare parts or customer support for that product\n    model, to give anyone who possesses the object code either (1) a\n    copy of the Corresponding Source for all the software in the\n    product that is covered by this License, on a durable physical\n    medium customarily used for software interchange, for a price no\n    more than your reasonable cost of physically performing this\n    conveying of source, or (2) access to copy the\n    Corresponding Source from a network server at no charge.\n\n    c) Convey individual copies of the object code with a copy of the\n    written offer to provide the Corresponding Source.  This\n    alternative is allowed only occasionally and noncommercially, and\n    only if you received the object code with such an offer, in accord\n    with subsection 6b.\n\n    d) Convey the object code by offering access from a designated\n    place (gratis or for a charge), and offer equivalent access to the\n    Corresponding Source in the same way through the same place at no\n    further charge.  You need not require recipients to copy the\n    Corresponding Source along with the object code.  If the place to\n    copy the object code is a network server, the Corresponding Source\n    may be on a different server (operated by you or a third party)\n    that supports equivalent copying facilities, provided you maintain\n    clear directions next to the object code saying where to find the\n    Corresponding Source.  Regardless of what server hosts the\n    Corresponding Source, you remain obligated to ensure that it is\n    available for as long as needed to satisfy these requirements.\n\n    e) Convey the object code using peer-to-peer transmission, provided\n    you inform other peers where the object code and Corresponding\n    Source of the work are being offered to the general public at no\n    charge under subsection 6d.\n\n  A separable portion of the object code, whose source code is excluded\nfrom the Corresponding Source as a System Library, need not be\nincluded in conveying the object code work.\n\n  A \"User Product\" is either (1) a \"consumer product\", which means any\ntangible personal property which is normally used for personal, family,\nor household purposes, or (2) anything designed or sold for incorporation\ninto a dwelling.  In determining whether a product is a consumer product,\ndoubtful cases shall be resolved in favor of coverage.  For a particular\nproduct received by a particular user, \"normally used\" refers to a\ntypical or common use of that class of product, regardless of the status\nof the particular user or of the way in which the particular user\nactually uses, or expects or is expected to use, the product.  A product\nis a consumer product regardless of whether the product has substantial\ncommercial, industrial or non-consumer uses, unless such uses represent\nthe only significant mode of use of the product.\n\n  \"Installation Information\" for a User Product means any methods,\nprocedures, authorization keys, or other information required to install\nand execute modified versions of a covered work in that User Product from\na modified version of its Corresponding Source.  The information must\nsuffice to ensure that the continued functioning of the modified object\ncode is in no case prevented or interfered with solely because\nmodification has been made.\n\n  If you convey an object code work under this section in, or with, or\nspecifically for use in, a User Product, and the conveying occurs as\npart of a transaction in which the right of possession and use of the\nUser Product is transferred to the recipient in perpetuity or for a\nfixed term (regardless of how the transaction is characterized), the\nCorresponding Source conveyed under this section must be accompanied\nby the Installation Information.  But this requirement does not apply\nif neither you nor any third party retains the ability to install\nmodified object code on the User Product (for example, the work has\nbeen installed in ROM).\n\n  The requirement to provide Installation Information does not include a\nrequirement to continue to provide support service, warranty, or updates\nfor a work that has been modified or installed by the recipient, or for\nthe User Product in which it has been modified or installed.  Access to a\nnetwork may be denied when the modification itself materially and\nadversely affects the operation of the network or violates the rules and\nprotocols for communication across the network.\n\n  Corresponding Source conveyed, and Installation Information provided,\nin accord with this section must be in a format that is publicly\ndocumented (and with an implementation available to the public in\nsource code form), and must require no special password or key for\nunpacking, reading or copying.\n\n  7. Additional Terms.\n\n  \"Additional permissions\" are terms that supplement the terms of this\nLicense by making exceptions from one or more of its conditions.\nAdditional permissions that are applicable to the entire Program shall\nbe treated as though they were included in this License, to the extent\nthat they are valid under applicable law.  If additional permissions\napply only to part of the Program, that part may be used separately\nunder those permissions, but the entire Program remains governed by\nthis License without regard to the additional permissions.\n\n  When you convey a copy of a covered work, you may at your option\nremove any additional permissions from that copy, or from any part of\nit.  (Additional permissions may be written to require their own\nremoval in certain cases when you modify the work.)  You may place\nadditional permissions on material, added by you to a covered work,\nfor which you have or can give appropriate copyright permission.\n\n  Notwithstanding any other provision of this License, for material you\nadd to a covered work, you may (if authorized by the copyright holders of\nthat material) supplement the terms of this License with terms:\n\n    a) Disclaiming warranty or limiting liability differently from the\n    terms of sections 15 and 16 of this License; or\n\n    b) Requiring preservation of specified reasonable legal notices or\n    author attributions in that material or in the Appropriate Legal\n    Notices displayed by works containing it; or\n\n    c) Prohibiting misrepresentation of the origin of that material, or\n    requiring that modified versions of such material be marked in\n    reasonable ways as different from the original version; or\n\n    d) Limiting the use for publicity purposes of names of licensors or\n    authors of the material; or\n\n    e) Declining to grant rights under trademark law for use of some\n    trade names, trademarks, or service marks; or\n\n    f) Requiring indemnification of licensors and authors of that\n    material by anyone who conveys the material (or modified versions of\n    it) with contractual assumptions of liability to the recipient, for\n    any liability that these contractual assumptions directly impose on\n    those licensors and authors.\n\n  All other non-permissive additional terms are considered \"further\nrestrictions\" within the meaning of section 10.  If the Program as you\nreceived it, or any part of it, contains a notice stating that it is\ngoverned by this License along with a term that is a further\nrestriction, you may remove that term.  If a license document contains\na further restriction but permits relicensing or conveying under this\nLicense, you may add to a covered work material governed by the terms\nof that license document, provided that the further restriction does\nnot survive such relicensing or conveying.\n\n  If you add terms to a covered work in accord with this section, you\nmust place, in the relevant source files, a statement of the\nadditional terms that apply to those files, or a notice indicating\nwhere to find the applicable terms.\n\n  Additional terms, permissive or non-permissive, may be stated in the\nform of a separately written license, or stated as exceptions;\nthe above requirements apply either way.\n\n  8. Termination.\n\n  You may not propagate or modify a covered work except as expressly\nprovided under this License.  Any attempt otherwise to propagate or\nmodify it is void, and will automatically terminate your rights under\nthis License (including any patent licenses granted under the third\nparagraph of section 11).\n\n  However, if you cease all violation of this License, then your\nlicense from a particular copyright holder is reinstated (a)\nprovisionally, unless and until the copyright holder explicitly and\nfinally terminates your license, and (b) permanently, if the copyright\nholder fails to notify you of the violation by some reasonable means\nprior to 60 days after the cessation.\n\n  Moreover, your license from a particular copyright holder is\nreinstated permanently if the copyright holder notifies you of the\nviolation by some reasonable means, this is the first time you have\nreceived notice of violation of this License (for any work) from that\ncopyright holder, and you cure the violation prior to 30 days after\nyour receipt of the notice.\n\n  Termination of your rights under this section does not terminate the\nlicenses of parties who have received copies or rights from you under\nthis License.  If your rights have been terminated and not permanently\nreinstated, you do not qualify to receive new licenses for the same\nmaterial under section 10.\n\n  9. Acceptance Not Required for Having Copies.\n\n  You are not required to accept this License in order to receive or\nrun a copy of the Program.  Ancillary propagation of a covered work\noccurring solely as a consequence of using peer-to-peer transmission\nto receive a copy likewise does not require acceptance.  However,\nnothing other than this License grants you permission to propagate or\nmodify any covered work.  These actions infringe copyright if you do\nnot accept this License.  Therefore, by modifying or propagating a\ncovered work, you indicate your acceptance of this License to do so.\n\n  10. Automatic Licensing of Downstream Recipients.\n\n  Each time you convey a covered work, the recipient automatically\nreceives a license from the original licensors, to run, modify and\npropagate that work, subject to this License.  You are not responsible\nfor enforcing compliance by third parties with this License.\n\n  An \"entity transaction\" is a transaction transferring control of an\norganization, or substantially all assets of one, or subdividing an\norganization, or merging organizations.  If propagation of a covered\nwork results from an entity transaction, each party to that\ntransaction who receives a copy of the work also receives whatever\nlicenses to the work the party's predecessor in interest had or could\ngive under the previous paragraph, plus a right to possession of the\nCorresponding Source of the work from the predecessor in interest, if\nthe predecessor has it or can get it with reasonable efforts.\n\n  You may not impose any further restrictions on the exercise of the\nrights granted or affirmed under this License.  For example, you may\nnot impose a license fee, royalty, or other charge for exercise of\nrights granted under this License, and you may not initiate litigation\n(including a cross-claim or counterclaim in a lawsuit) alleging that\nany patent claim is infringed by making, using, selling, offering for\nsale, or importing the Program or any portion of it.\n\n  11. Patents.\n\n  A \"contributor\" is a copyright holder who authorizes use under this\nLicense of the Program or a work on which the Program is based.  The\nwork thus licensed is called the contributor's \"contributor version\".\n\n  A contributor's \"essential patent claims\" are all patent claims\nowned or controlled by the contributor, whether already acquired or\nhereafter acquired, that would be infringed by some manner, permitted\nby this License, of making, using, or selling its contributor version,\nbut do not include claims that would be infringed only as a\nconsequence of further modification of the contributor version.  For\npurposes of this definition, \"control\" includes the right to grant\npatent sublicenses in a manner consistent with the requirements of\nthis License.\n\n  Each contributor grants you a non-exclusive, worldwide, royalty-free\npatent license under the contributor's essential patent claims, to\nmake, use, sell, offer for sale, import and otherwise run, modify and\npropagate the contents of its contributor version.\n\n  In the following three paragraphs, a \"patent license\" is any express\nagreement or commitment, however denominated, not to enforce a patent\n(such as an express permission to practice a patent or covenant not to\nsue for patent infringement).  To \"grant\" such a patent license to a\nparty means to make such an agreement or commitment not to enforce a\npatent against the party.\n\n  If you convey a covered work, knowingly relying on a patent license,\nand the Corresponding Source of the work is not available for anyone\nto copy, free of charge and under the terms of this License, through a\npublicly available network server or other readily accessible means,\nthen you must either (1) cause the Corresponding Source to be so\navailable, or (2) arrange to deprive yourself of the benefit of the\npatent license for this particular work, or (3) arrange, in a manner\nconsistent with the requirements of this License, to extend the patent\nlicense to downstream recipients.  \"Knowingly relying\" means you have\nactual knowledge that, but for the patent license, your conveying the\ncovered work in a country, or your recipient's use of the covered work\nin a country, would infringe one or more identifiable patents in that\ncountry that you have reason to believe are valid.\n\n  If, pursuant to or in connection with a single transaction or\narrangement, you convey, or propagate by procuring conveyance of, a\ncovered work, and grant a patent license to some of the parties\nreceiving the covered work authorizing them to use, propagate, modify\nor convey a specific copy of the covered work, then the patent license\nyou grant is automatically extended to all recipients of the covered\nwork and works based on it.\n\n  A patent license is \"discriminatory\" if it does not include within\nthe scope of its coverage, prohibits the exercise of, or is\nconditioned on the non-exercise of one or more of the rights that are\nspecifically granted under this License.  You may not convey a covered\nwork if you are a party to an arrangement with a third party that is\nin the business of distributing software, under which you make payment\nto the third party based on the extent of your activity of conveying\nthe work, and under which the third party grants, to any of the\nparties who would receive the covered work from you, a discriminatory\npatent license (a) in connection with copies of the covered work\nconveyed by you (or copies made from those copies), or (b) primarily\nfor and in connection with specific products or compilations that\ncontain the covered work, unless you entered into that arrangement,\nor that patent license was granted, prior to 28 March 2007.\n\n  Nothing in this License shall be construed as excluding or limiting\nany implied license or other defenses to infringement that may\notherwise be available to you under applicable patent law.\n\n  12. No Surrender of Others' Freedom.\n\n  If conditions are imposed on you (whether by court order, agreement or\notherwise) that contradict the conditions of this License, they do not\nexcuse you from the conditions of this License.  If you cannot convey a\ncovered work so as to satisfy simultaneously your obligations under this\nLicense and any other pertinent obligations, then as a consequence you may\nnot convey it at all.  For example, if you agree to terms that obligate you\nto collect a royalty for further conveying from those to whom you convey\nthe Program, the only way you could satisfy both those terms and this\nLicense would be to refrain entirely from conveying the Program.\n\n  13. Use with the GNU Affero General Public License.\n\n  Notwithstanding any other provision of this License, you have\npermission to link or combine any covered work with a work licensed\nunder version 3 of the GNU Affero General Public License into a single\ncombined work, and to convey the resulting work.  The terms of this\nLicense will continue to apply to the part which is the covered work,\nbut the special requirements of the GNU Affero General Public License,\nsection 13, concerning interaction through a network will apply to the\ncombination as such.\n\n  14. Revised Versions of this License.\n\n  The Free Software Foundation may publish revised and/or new versions of\nthe GNU General Public License from time to time.  Such new versions will\nbe similar in spirit to the present version, but may differ in detail to\naddress new problems or concerns.\n\n  Each version is given a distinguishing version number.  If the\nProgram specifies that a certain numbered version of the GNU General\nPublic License \"or any later version\" applies to it, you have the\noption of following the terms and conditions either of that numbered\nversion or of any later version published by the Free Software\nFoundation.  If the Program does not specify a version number of the\nGNU General Public License, you may choose any version ever published\nby the Free Software Foundation.\n\n  If the Program specifies that a proxy can decide which future\nversions of the GNU General Public License can be used, that proxy's\npublic statement of acceptance of a version permanently authorizes you\nto choose that version for the Program.\n\n  Later license versions may give you additional or different\npermissions.  However, no additional obligations are imposed on any\nauthor or copyright holder as a result of your choosing to follow a\nlater version.\n\n  15. Disclaimer of Warranty.\n\n  THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY\nAPPLICABLE LAW.  EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT\nHOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM \"AS IS\" WITHOUT WARRANTY\nOF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO,\nTHE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR\nPURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM\nIS WITH YOU.  SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF\nALL NECESSARY SERVICING, REPAIR OR CORRECTION.\n\n  16. Limitation of Liability.\n\n  IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING\nWILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS\nTHE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY\nGENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE\nUSE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF\nDATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD\nPARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS),\nEVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF\nSUCH DAMAGES.\n\n  17. Interpretation of Sections 15 and 16.\n\n  If the disclaimer of warranty and limitation of liability provided\nabove cannot be given local legal effect according to their terms,\nreviewing courts shall apply local law that most closely approximates\nan absolute waiver of all civil liability in connection with the\nProgram, unless a warranty or assumption of liability accompanies a\ncopy of the Program in return for a fee.\n\n                     END OF TERMS AND CONDITIONS\n\n            How to Apply These Terms to Your New Programs\n\n  If you develop a new program, and you want it to be of the greatest\npossible use to the public, the best way to achieve this is to make it\nfree software which everyone can redistribute and change under these terms.\n\n  To do so, attach the following notices to the program.  It is safest\nto attach them to the start of each source file to most effectively\nstate the exclusion of warranty; and each file should have at least\nthe \"copyright\" line and a pointer to where the full notice is found.\n\n    <one line to give the program's name and a brief idea of what it does.>\n    Copyright (C) <year>  <name of author>\n\n    This program is free software: you can redistribute it and/or modify\n    it under the terms of the GNU General Public License as published by\n    the Free Software Foundation, either version 3 of the License, or\n    (at your option) any later version.\n\n    This program is distributed in the hope that it will be useful,\n    but WITHOUT ANY WARRANTY; without even the implied warranty of\n    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n    GNU General Public License for more details.\n\n    You should have received a copy of the GNU General Public License\n    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n\nAlso add information on how to contact you by electronic and paper mail.\n\n  If the program does terminal interaction, make it output a short\nnotice like this when it starts in an interactive mode:\n\n    <program>  Copyright (C) <year>  <name of author>\n    This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'.\n    This is free software, and you are welcome to redistribute it\n    under certain conditions; type `show c' for details.\n\nThe hypothetical commands `show w' and `show c' should show the appropriate\nparts of the General Public License.  Of course, your program's commands\nmight be different; for a GUI interface, you would use an \"about box\".\n\n  You should also get your employer (if you work as a programmer) or school,\nif any, to sign a \"copyright disclaimer\" for the program, if necessary.\nFor more information on this, and how to apply and follow the GNU GPL, see\n<https://www.gnu.org/licenses/>.\n\n  The GNU General Public License does not permit incorporating your program\ninto proprietary programs.  If your program is a subroutine library, you\nmay consider it more useful to permit linking proprietary applications with\nthe library.  If this is what you want to do, use the GNU Lesser General\nPublic License instead of this License.  But first, please read\n<https://www.gnu.org/licenses/why-not-lgpl.html>.\n"
  },
  {
    "path": "feature_tracker/include/Color.h",
    "content": "/**   This File is part of Feature Tracker\n *  \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#pragma once\n\n#include <iostream>\n\nnamespace color\n{\n    inline std::ostream& setBlue(std::ostream& s)\n    {\n        s << \"\\033[0;1;34m\";\n        return s;\n    }\n    \n    inline std::ostream& setRed(std::ostream& s)\n    {\n        s << \"\\033[0;1;31m\";\n        return s;\n    }\n    \n    inline std::ostream& setGreen(std::ostream& s)\n    {\n        s << \"\\033[0;1;32m\";\n        return s;\n    }\n    \n    inline std::ostream& setYellow(std::ostream& s)\n    {\n        s << \"\\033[0;1;33m\";\n        return s;\n    }\n    \n    inline std::ostream& setWhite(std::ostream& s)\n    {\n        s << \"\\033[0;1;37m\";\n        return s;\n    }\n    \n    inline std::ostream& resetColor(std::ostream& s)\n    {\n        s << \"\\033[0m\";\n        return s;\n    }\n}\n"
  },
  {
    "path": "feature_tracker/include/mono_parameters.h",
    "content": "/**   This File is part of Feature Tracker\n *  \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n *    \n *    A few functions of the mono tracker mimic the realization from GVINS\n *    <https://github.com/HKUST-Aerial-Robotics/GVINS>\n *    GVINS is under GPLv3 License.\n */\n \n#pragma once\n\n#include <ros/ros.h>\n#include <opencv2/highgui/highgui.hpp>\n\nnamespace feature_tracker\n{\n    class MonoParameters\n    {\n    public:\n        MonoParameters();\n        \n        int row, col;\n        int focal_length;\n        \n        std::string image_topic;\n        std::string imu_topic;\n        std::string cam_name;\n        std::string cam_path;\n        \n        int max_cnt;\n        int min_dist;\n        \n        int window_size;\n        int freq;\n        \n        double f_threshold;\n        \n        int show_track;\n        int equalize;\n        double timer_warning_thres;\n        int show_timer;\n        \n        void readParameters(ros::NodeHandle& n);\n        \n        template <typename T>\n        static T readParam(ros::NodeHandle& n, std::string name);\n    };\n    \n    template <typename T>\n    T MonoParameters::readParam(ros::NodeHandle& n, std::string name)\n    {\n        T ans;\n        if (n.getParam(name, ans))\n        {\n            ROS_INFO_STREAM(\"Loaded \" << name << \": \" << ans);    \n        }\n        else\n        {\n            ROS_ERROR_STREAM(\"Failed to load: \" << name);\n            n.shutdown();\n        }\n        \n        return ans;\n    }\n\n}\n\n"
  },
  {
    "path": "feature_tracker/include/mono_tracker.h",
    "content": "/**   This File is part of Feature Tracker\n *  \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n *    \n *    A few functions of the mono tracker mimic the realization from GVINS\n *    <https://github.com/HKUST-Aerial-Robotics/GVINS>\n *    GVINS is under GPLv3 License.\n */\n \n\n#pragma once\n\n#include <ros/ros.h>\n#include <sensor_msgs/Image.h>\n#include <sensor_msgs/image_encodings.h>\n\n#include <cv_bridge/cv_bridge.h>\n#include <message_filters/subscriber.h>\n\n#include <iostream>\n#include <string>\n#include <vector>\n#include <map>\n\n#include <opencv2/opencv.hpp>\n\n#include <Eigen/Core>\n#include <Eigen/Geometry>\n\n#include \"tic_toc.hpp\"\n\n#include \"camera_factory.h\"\n#include \"MeiCamera.h\"\n#include \"PinholeCamera.h\"\n\n#include \"mono_parameters.h\"\n\n#include <feature_tracker/MonoMeas.h>\n#include <feature_tracker/MonoFrame.h>\n\nnamespace feature_tracker\n{\n    class MonoTracker\n    {\n    public:\n        EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n        \n        MonoTracker(ros::NodeHandle& n) : nh(n) {}\n        ~MonoTracker() {}\n        MonoTracker(const MonoTracker&) = delete;\n        MonoTracker operator=(const MonoTracker&) = delete;\n        \n        void init();\n        \n    private:\n        ros::NodeHandle nh;\n        ros::Publisher pub_mono_features, pub_match;\n        ros::Subscriber sub_img;\n        void img_callback(const sensor_msgs::ImageConstPtr& img_msg);\n        \n        MonoParameters param;\n        camera_model::CameraPtr m_camera;        \n        \n        bool pub_this_frame;\n        bool first_image_flag;\n        double first_image_time, last_image_time;\n        int pub_count;\n        \n        static int n_id;\n        \n        double cur_time, prev_time;\n        cv::Mat mask;\n        cv::Mat prev_img, cur_img, forw_img;\n        \n        std::vector<cv::Point2f> prev_pts, cur_pts, forw_pts;\n        std::vector<cv::Point2f> prev_un_pts, cur_un_pts;\n        std::vector<cv::Point2f> pts_velocity;\n        std::vector<cv::Point2f> n_pts;\n        \n        std::vector<int> ids;\n        std::vector<int> track_cnt;\n        \n        std::map<int, cv::Point2f> cur_un_pts_map;\n        std::map<int, cv::Point2f> prev_un_pts_map;\n        \n        \n        void readImage(const cv::Mat& _img, double _cur_time);\n        \n        void rejectWithF();\n        \n        void setMask();\n        \n        void addPoints();\n        \n        void undistortedPoints();\n        \n        void updateID();\n        \n        \n        bool inBorder(const cv::Point2f& pt);\n        \n        template <typename T1, typename T2 = uchar>\n        void reduceVector(std::vector<T1>& v, std::vector<T2> status);\n        \n        void readIntrinsicParameter(const std::string& calib_file);\n    };\n    \n    typedef boost::shared_ptr<MonoTracker> MonoTrackerPtr; \n    \n    template <typename T1, typename T2>\n    void MonoTracker::reduceVector(std::vector<T1>& v, std::vector<T2> status)\n    {\n        int j = 0;\n        for (int i = 0; i < int(v.size()); ++i)\n            if (status[i])\n                v[j++] = v[i];\n            \n        v.resize(j);\n    }\n}\n"
  },
  {
    "path": "feature_tracker/include/random_pair.hpp",
    "content": "/**   This File is part of Feature Tracker\n *  \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#pragma once\n\n#include <vector>\n#include <ctime>\n#include <algorithm>\n#include <cmath>\n\nnamespace feature_tracker\n{\n    class RandomPairGenerator\n    {\n    public:\n        RandomPairGenerator(unsigned int n = 50) : xa(static_cast<int>(n*(n-1)/2)), count(0), N(n) \n        {\n            for (int i = 0; i < static_cast<int>(n*(n-1)/2); ++i)\n                xa[i] = i;\n            std::random_shuffle(xa.begin(), xa.end());\n        }\n        RandomPairGenerator(RandomPairGenerator& obj) = delete;\n        ~RandomPairGenerator(){}\n        \n        static void setRandomPairSeed()\n        {\n            srand((unsigned)time(NULL));\n        }\n        \n        void generatePair(std::vector<unsigned int>& pair)\n        {\n            pair.clear();\n            unsigned int i = xa[count];\n            \n            float k_max = (1.0+sqrt(i*8+9))/2.0;\n            float k_min = (-1.0+sqrt(i*8+9))/2.0;\n            \n            float k_guess = floor(k_max);\n            unsigned int k;\n            \n            if (k_guess < k_max && k_guess >= k_min)\n                k = static_cast<unsigned int>(k_guess);\n            else\n                k = static_cast<unsigned int>(k_guess)-1;\n            \n            pair.push_back(N-k-1);\n            pair.push_back(N-k+i-static_cast<unsigned int>(k*(k-1)/2));\n            \n            ++count;\n            if (count >= static_cast<int>(N*(N-1)/2)) count = 0;\n        }\n    private:\n        std::vector<unsigned int> xa;\n        unsigned int count;\n        unsigned int N;\n    };\n}\n\n"
  },
  {
    "path": "feature_tracker/include/stereo_parameters.h",
    "content": "/**   This File is part of Feature Tracker\n *  \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#pragma once\n\n#include <ros/ros.h>\n#include <opencv2/highgui/highgui.hpp>\n\n#include <Eigen/Core>\n#include <Eigen/Geometry>\n\nnamespace feature_tracker\n{\n    class StereoParameters\n    {\n    public:\n        EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n        \n        StereoParameters();\n        \n        int row, col;\n        \n        std::string image_left_topic;\n        std::string image_right_topic;\n        \n        std::string cam_left_path;\n        std::string cam_right_path;\n        \n        std::string cam_left_name;\n        std::string cam_right_name;\n        \n        int max_cnt;\n        int min_dist;\n        \n        int window_size;\n        int freq;\n        \n        int show_track;\n        \n        int show_timer;\n        double timer_warning_thres;\n        \n        int equalize;\n        \n        double epipolar_thres;\n        double F_threshold;\n        \n        int print_track_info;\n        \n        Eigen::Isometry3d T_cl2i, T_cr2i;\n        \n        void readParameters(ros::NodeHandle& n);\n        \n        template <typename T>\n        static T readParam(ros::NodeHandle& n, std::string name);\n    };\n    \n    template <typename T>\n    T StereoParameters::readParam(ros::NodeHandle& n, std::string name)\n    {\n        T ans;\n        if (n.getParam(name, ans))\n        {\n            ROS_INFO_STREAM(\"Loaded \" << name << \": \" << ans);    \n        }\n        else\n        {\n            ROS_ERROR_STREAM(\"Failed to load: \" << name);\n            n.shutdown();\n        }\n        \n        return ans;\n    }\n    \n}\n"
  },
  {
    "path": "feature_tracker/include/stereo_tracker.h",
    "content": "/**   This File is part of Feature Tracker\n *  \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#pragma once\n\n#include <ros/ros.h>\n#include <sensor_msgs/Image.h>\n#include <sensor_msgs/image_encodings.h>\n\n#include <cv_bridge/cv_bridge.h>\n\n#include <message_filters/subscriber.h>\n#include <message_filters/synchronizer.h>\n#include <message_filters/sync_policies/approximate_time.h>\n\n#include <iostream>\n#include <string>\n#include <vector>\n#include <map>\n\n#include <opencv2/opencv.hpp>\n\n#include <Eigen/Core>\n#include <Eigen/Geometry>\n\n#include \"tic_toc.hpp\"\n\n#include \"camera_factory.h\"\n#include \"MeiCamera.h\"\n#include \"PinholeCamera.h\"\n\n#include \"random_pair.hpp\"\n#include \"stereo_parameters.h\"\n\n#include \"feature_tracker/StereoMeas.h\"\n#include \"feature_tracker/StereoFrame.h\"\n\nnamespace feature_tracker\n{\n    class StereoTracker\n    {\n    public:\n        EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n        \n        StereoTracker(ros::NodeHandle& n) : nh(n) {}\n        ~StereoTracker() {}\n        StereoTracker(const StereoTracker&) = delete;\n        StereoTracker operator=(const StereoTracker&) = delete;\n        \n        void init();\n        \n    private:\n        ros::NodeHandle nh;\n        ros::Publisher pub_stereo_feature, pub_match;\n        \n        typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> stereoSyncPolicy;\n        boost::shared_ptr<message_filters::Subscriber<sensor_msgs::Image>> cam_left_sub_;\n        boost::shared_ptr<message_filters::Subscriber<sensor_msgs::Image>> cam_right_sub_;\n        boost::shared_ptr<message_filters::Synchronizer<stereoSyncPolicy>> stereo_sub_;\n        \n        StereoParameters param;\n        camera_model::CameraPtr m_camera_left, m_camera_right;\n        \n        cv::Mat mask;\n        bool first_image_flag;\n        double first_image_time, last_image_time;\n        bool pub_this_frame;\n        unsigned long long int pub_count;\n        \n        double cur_time, prev_time;\n        cv::Mat prev_left_img, cur_left_img, forw_left_img;\n        cv::Mat prev_right_img, cur_right_img, forw_right_img;\n        \n        std::vector<cv::Point2f> prev_left_pts, cur_left_pts, forw_left_pts;\n        std::vector<cv::Point2f> prev_right_pts, cur_right_pts, forw_right_pts;\n        \n        std::vector<cv::Point2f> prev_left_un_pts, cur_left_un_pts;\n        std::vector<cv::Point2f> prev_right_un_pts, cur_right_un_pts;\n        \n        std::vector<unsigned long long int> ids;\n        std::vector<unsigned int> track_cnt;\n        \n        std::vector<cv::Point2f> n_left_pts, n_right_pts;\n        \n        static unsigned long long int n_id;\n        \n        \n        void stereo_callback(const sensor_msgs::Image::ConstPtr& cam_left_msg, const sensor_msgs::Image::ConstPtr& cam_right_msg);\n        \n        void readIntrinsicParameters();\n        \n        void processStereoImage(const cv::Mat& _left_img, const cv::Mat& _right_img, double _cur_time);\n        \n        bool inBorder(const cv::Point2f& pt);\n        \n        void markStereoMatchOutlier(const std::vector<cv::Point2f>& left_pts, const std::vector<cv::Point2f>& right_pts, std::vector<uchar>& status);\n        \n        void rejectWithTwinRANSAC();\n        \n        void setMask();\n        \n        void addPoints();\n        \n        void undistortedPoints();\n        \n        void updateID();\n        \n        static void publishStereoImg(const cv::Mat& left_img, const cv::Mat& right_img, const std::vector<cv::Point2f>& left_pts, const std::vector<cv::Point2f>& right_pts, const int row, const int col, bool pair_line = true);\n        \n        void predictHomography(const std::vector<cv::Point2f>& left_pts, std::vector<cv::Point2f>& right_pts);\n        \n        template <typename T1, typename T2 = uchar>\n        void reduceVector(std::vector<T1>& v, std::vector<T2> status);\n        \n        template <typename T1, typename T2 = uchar>\n        void reduceTwinVector(std::vector<T1>& v1, std::vector<T1>& v2, std::vector<T2> status);\n        \n        template <typename T1, typename T2 = uchar>\n        void reduceQuadVector(std::vector<T1>& v1, std::vector<T1>& v2, std::vector<T1>& v3, std::vector<T1>& v4, std::vector<T2> status);\n    };\n    \n    typedef boost::shared_ptr<StereoTracker> StereoTrackerPtr;\n    \n    template <typename T1, typename T2>\n    void StereoTracker::reduceVector(std::vector<T1>& v, std::vector<T2> status)\n    {\n        int j = 0;\n        for (int i = 0; i < int(v.size()); ++i)\n            if (status[i])\n                v[j++] = v[i];\n            \n        v.resize(j);\n    }    \n    \n    template <typename T1, typename T2>\n    void StereoTracker::reduceTwinVector(std::vector<T1>& v1, std::vector<T1>& v2, std::vector<T2> status)\n    {\n        if (v1.size() != v2.size())\n            ROS_WARN(\"[Stereo Tracker]: Reduce twin vectors, size: %i and %i are not equal!\", int(v1.size()), int(v2.size()));\n        \n        int j = 0;\n        for (int i = 0; i < int(v1.size()); ++i)\n            if (status[i])\n            {\n                v1[j] = v1[i];\n                v2[j] = v2[i];\n                ++j;\n            }\n            \n        v1.resize(j);\n        v2.resize(j);        \n    }\n    \n    template <typename T1, typename T2>\n    void StereoTracker::reduceQuadVector(std::vector<T1>& v1, std::vector<T1>& v2, std::vector<T1>& v3, std::vector<T1>& v4, std::vector<T2> status)\n    {\n        if (!(v1.size() == v2.size() && v2.size() == v3.size() && v3.size() == v4.size()))\n            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())); \n        \n        int j = 0;\n        for (int i = 0; i < int(v1.size()); ++i)\n            if (status[i])\n            {\n                v1[j] = v1[i];\n                v2[j] = v2[i];\n                v3[j] = v3[i];\n                v4[j] = v4[i];\n                ++j;\n            }\n            \n        v1.resize(j);\n        v2.resize(j);         \n        v3.resize(j);\n        v4.resize(j);\n    }\n}\n"
  },
  {
    "path": "feature_tracker/include/tic_toc.hpp",
    "content": "/**   This File is part of Feature Tracker\n *  \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n *    \n *    A few functions of the mono tracker mimic the realization from GVINS\n *    <https://github.com/HKUST-Aerial-Robotics/GVINS>\n *    GVINS is under GPLv3 License.\n */\n\n#pragma once\n\n#include <ctime>\n#include <cstdlib>\n#include <chrono>\n\nnamespace feature_tracker\n{\n    class TicToc\n    {\n    public:\n        TicToc()\n        {\n            tic();\n        }\n        \n        void tic()\n        {\n            start = std::chrono::system_clock::now();\n        }\n        \n        double toc()\n        {\n            end = std::chrono::system_clock::now();\n            std::chrono::duration<double> elapsed_seconds = end - start;\n            return elapsed_seconds.count() * 1000.0;\n        }\n        \n    private:\n        std::chrono::time_point<std::chrono::system_clock> start, end;\n    };\n}\n\n"
  },
  {
    "path": "feature_tracker/launch/sportsfield/mono_tracker_sf.launch",
    "content": "<launch>\n    <arg name=\"config_path\" default = \"$(find feature_tracker)/../config/sportsfield/mono_config.yaml\" />\n    \n    <node name=\"mono_tracker\" pkg=\"feature_tracker\" type=\"mono_tracker\" output=\"screen\">\n        <param name=\"config_file\" type=\"string\" value=\"$(arg config_path)\" />\n    </node>\n\n\n\n</launch>\n"
  },
  {
    "path": "feature_tracker/launch/sportsfield/stereo_tracker_sf.launch",
    "content": "<launch>\n    <arg name=\"config_path_left\" default = \"$(find feature_tracker)/../config/sportsfield/stereo_left_config.yaml\" />\n    <arg name=\"config_path_right\" default = \"$(find feature_tracker)/../config/sportsfield/stereo_right_config.yaml\" />\n    \n    <node name=\"stereo_tracker\" pkg=\"feature_tracker\" type=\"stereo_tracker\" output=\"screen\">\n        <param name=\"config_file_left\" type=\"string\" value=\"$(arg config_path_left)\" />\n        <param name=\"config_file_right\" type=\"string\" value=\"$(arg config_path_right)\" />\n    </node>\n\n\n</launch>\n"
  },
  {
    "path": "feature_tracker/msg/MonoFrame.msg",
    "content": "std_msgs/Header header\nMonoMeas[] mono_features\n"
  },
  {
    "path": "feature_tracker/msg/MonoMeas.msg",
    "content": "uint64 id\nfloat64 u0\nfloat64 v0\n"
  },
  {
    "path": "feature_tracker/msg/StereoFrame.msg",
    "content": "std_msgs/Header header\nStereoMeas[] stereo_features\n"
  },
  {
    "path": "feature_tracker/msg/StereoMeas.msg",
    "content": "uint64 id\nfloat64 u0\nfloat64 v0\nfloat64 u1\nfloat64 v1\n"
  },
  {
    "path": "feature_tracker/package.xml",
    "content": "<?xml version=\"1.0\"?>\n<package format=\"2\">\n  <name>feature_tracker</name>\n  <version>0.0.0</version>\n  <description>The mono and stereo feature_tracker package</description>\n\n  <maintainer email=\"lcw18@mails.tsinghua.edu.cn\">lcw18</maintainer>\n\n\n  <!-- One license tag required, multiple allowed, one license per tag -->\n  <!-- Commonly used license strings: -->\n  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->\n  <license>GPLv3</license>\n\n  <buildtool_depend>catkin</buildtool_depend>\n\n\n  <build_depend>roscpp</build_depend>\n  <build_depend>std_msgs</build_depend>\n\n  <build_export_depend>roscpp</build_export_depend>\n  <build_export_depend>std_msgs</build_export_depend>\n \n  <exec_depend>nodelet</exec_depend>\n  <exec_depend>pluginlib</exec_depend>\n  <exec_depend>roscpp</exec_depend>\n  <exec_depend>std_msgs</exec_depend>\n  <depend>roslib</depend>\n  <depend>rosbag</depend>\n  <depend>message_filters</depend>\n  <depend>sensor_msgs</depend>\n  <depend>eigen_conversions</depend>\n  <depend>cv_bridge</depend>\n  <depend>image_transport</depend>\n  <build_depend>message_generation</build_depend>\n  <exec_depend>message_runtime</exec_depend>\n\n\n  <!-- The export tag contains other, unspecified, tags -->\n  <export>\n    <!-- You can specify that this package is a metapackage here: -->\n    <!-- <metapackage/> -->\n\n    <!-- Other tools can request additional information be placed here -->\n\n  </export>\n</package>\n"
  },
  {
    "path": "feature_tracker/src/mono_parameters.cpp",
    "content": "/**   This File is part of Feature Tracker\n *  \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n *    \n *    A few functions of the mono tracker mimic the realization from GVINS\n *    <https://github.com/HKUST-Aerial-Robotics/GVINS>\n *    GVINS is under GPLv3 License.\n */\n \n#include \"mono_parameters.h\"\n\nnamespace feature_tracker\n{\n    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)\n    {}\n    \n    void MonoParameters::readParameters(ros::NodeHandle& n)\n    {\n        std::string config_file;\n        config_file = readParam<std::string>(n, \"config_file\");\n        \n        cv::FileStorage fs(config_file, cv::FileStorage::READ);\n        if (!fs.isOpened())\n        {\n            ROS_ERROR(\"ERROR: Wrong path to mono config yaml files!\");\n            n.shutdown();\n        }\n        \n        cam_path = config_file;\n        \n        row = fs[\"image_height\"];\n        col = fs[\"image_width\"];\n        focal_length = 460;\n        fs[\"image_topic\"] >> image_topic;\n        fs[\"imu_topic\"] >> imu_topic;\n        fs[\"camera_name\"] >> cam_name;\n        max_cnt = fs[\"max_cnt\"];\n        min_dist = fs[\"min_dist\"];\n        \n        window_size = 20;\n        \n        freq = fs[\"freq\"];\n        if (freq == 0) freq = 100;\n        \n        f_threshold = fs[\"F_threshold\"];\n        show_track = fs[\"show_track\"];\n        show_timer = fs[\"show_timer\"];\n        timer_warning_thres = fs[\"timer_warning_thres\"];\n        equalize = fs[\"equalize\"];\n        \n        fs.release();\n    }\n}\n"
  },
  {
    "path": "feature_tracker/src/mono_tracker.cpp",
    "content": "/**   This File is part of Feature Tracker\n *  \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n *    \n *    A few functions of the mono tracker mimic the realization from GVINS\n *    <https://github.com/HKUST-Aerial-Robotics/GVINS>\n *    GVINS is under GPLv3 License.\n */\n\n#include \"mono_tracker.h\"\n\n#include \"Color.h\"\n\nnamespace feature_tracker\n{\n    int MonoTracker::n_id = 0;\n    \n    void MonoTracker::init()\n    {\n        param.readParameters(nh);\n        \n        readIntrinsicParameter(param.cam_path);\n        \n        sub_img = nh.subscribe(param.image_topic, 100, &MonoTracker::img_callback, this);\n        pub_mono_features = nh.advertise<feature_tracker::MonoFrame>(\"mono_feature\", 100);\n        pub_match = nh.advertise<sensor_msgs::Image>(\"mono_track\", 100);\n        \n        first_image_flag = true;\n        first_image_time = 0.0;\n        last_image_time = 0.0;\n        \n        pub_this_frame = false;\n        \n        pub_count = 1;\n    }\n    \n    void MonoTracker::img_callback(const sensor_msgs::ImageConstPtr& img_msg)\n    {\n        TicToc mono_timer;\n        \n        if (first_image_flag)\n        {\n            first_image_flag = false;\n            first_image_time = img_msg->header.stamp.toSec();\n            last_image_time = img_msg->header.stamp.toSec();\n            return;\n        }\n        \n        if (img_msg->header.stamp.toSec() - last_image_time > 1.0 || img_msg->header.stamp.toSec() < last_image_time)\n        {\n            ROS_WARN(\"[Mono Tracker]: Image discontinue! Mono tracker reset!\");\n            first_image_flag = true;\n            first_image_time = 0.0;\n            last_image_time = 0.0;\n            pub_count = 1;\n            pub_this_frame = false;\n            return;\n        }\n        \n        last_image_time = img_msg->header.stamp.toSec();\n        \n        if (std::round(1.0*pub_count/(last_image_time-first_image_time)) <= param.freq)\n        {\n            pub_this_frame = true;\n            \n            if (std::fabs(1.0*pub_count/(last_image_time-first_image_time)-param.freq) < 0.01*param.freq)\n            {\n                first_image_time = last_image_time;\n                pub_count = 0;\n            }\n        }\n        else\n        {\n            pub_this_frame = false;\n        }\n        \n        \n        cv_bridge::CvImageConstPtr ptr;\n        if (img_msg->encoding == \"8UC1\")\n        {\n            sensor_msgs::Image img;\n            img.header = img_msg->header;\n            img.height = img_msg->height;\n            img.width = img_msg->width;\n            img.is_bigendian = img_msg->is_bigendian;\n            img.step = img_msg->step;\n            img.data = img_msg->data;\n            img.encoding = \"mono8\";\n            ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8);\n        }\n        else\n            ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8);\n        \n        \n        readImage(ptr->image, img_msg->header.stamp.toSec());\n        \n        updateID();\n        \n        if (pub_this_frame)\n        {\n            ++pub_count;\n            \n            feature_tracker::MonoFrame::Ptr mono_frame_ptr(new feature_tracker::MonoFrame);\n            mono_frame_ptr->header = img_msg->header;\n            \n            for (int i = 0; i < ids.size(); ++i)\n            {\n                if (track_cnt[i] > 1)\n                {\n                    feature_tracker::MonoMeas::Ptr mono_feature_ptr(new feature_tracker::MonoMeas);\n                    \n                    mono_feature_ptr->id = ids[i];\n                    mono_feature_ptr->u0 = cur_un_pts[i].x;\n                    mono_feature_ptr->v0 = cur_un_pts[i].y;\n                    \n                    mono_frame_ptr->mono_features.push_back(*mono_feature_ptr);\n                }\n            }\n            \n            if (mono_frame_ptr->mono_features.size() > 0)\n                pub_mono_features.publish(mono_frame_ptr);\n            \n            if (param.show_track)\n            {\n                ptr = cv_bridge::cvtColor(ptr, sensor_msgs::image_encodings::BGR8);\n                \n                cv::Mat tmp_img = ptr->image;\n                \n                for (unsigned int j = 0; j < cur_pts.size(); ++j)\n                {\n                    double len = std::min(1.0, 1.0*track_cnt[j]/param.window_size);\n                    cv::circle(tmp_img, cur_pts[j], 2, cv::Scalar(255*(1-len), 0, 255*len), 2);\n                }\n                pub_match.publish(ptr->toImageMsg());\n            }\n        }\n        \n        if (param.show_timer)\n        {\n            double mono_time = mono_timer.toc();\n            \n            if (mono_time <= param.timer_warning_thres)\n                std::cout << color::setBlue << \"[MonoTracker]: Mono tracking processing time = \" << mono_time << \" (ms) \" << color::resetColor << std::endl;\n            else\n                std::cout << color::setRed << \"[MonoTracker]: Mono tracking processing time = \" << mono_time << \" (ms) \" << color::resetColor << std::endl;\n        }\n        \n    }\n    \n    void MonoTracker::updateID()\n    {\n        for (unsigned int i = 0; i < ids.size(); ++i)\n            if (ids[i] == -1)\n                ids[i] = n_id++;\n    }\n    \n    \n    void MonoTracker::readImage(const cv::Mat& _img, double _cur_time)\n    {\n        cv::Mat img;\n        cur_time = _cur_time;\n        \n        if (param.equalize)\n        {\n            cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));\n            clahe->apply(_img, img);\n        }\n        else\n            img = _img;\n            \n        if (forw_img.empty())\n            prev_img = cur_img = forw_img = img;\n        else\n            forw_img = img;\n            \n        forw_pts.clear();\n        \n        if (cur_pts.size() > 0)\n        {\n            std::vector<uchar> status;\n            std::vector<float> err;\n            \n            cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);\n            \n            for (int i = 0; i < int(forw_pts.size()); ++i)\n                if (status[i] && !inBorder(forw_pts[i]))\n                    status[i] = 0;\n                \n            reduceVector<cv::Point2f>(prev_pts, status);\n            reduceVector<cv::Point2f>(cur_pts, status);\n            reduceVector<cv::Point2f>(forw_pts, status);\n            reduceVector<int>(ids, status);\n            reduceVector<cv::Point2f>(cur_un_pts, status);\n            reduceVector<int>(track_cnt, status);\n        }\n        \n        for (auto& n: track_cnt)\n            ++n;\n        \n        if (pub_this_frame)\n        {\n            rejectWithF();\n            \n            setMask();\n            \n            int n_max_cnt = param.max_cnt - static_cast<int>(forw_pts.size());\n            \n            if (n_max_cnt > 0)\n            {\n                if (mask.empty())\n                    ROS_WARN(\"[Mono Tracker]: Mask is empty!\");\n                if (mask.type() != CV_8UC1)\n                    ROS_WARN(\"[Mono Tracker]: Mask type wrong!\");\n                if (mask.size() != forw_img.size())\n                    ROS_WARN(\"[Mono Tracker]: Mask size wrong!\");\n                \n                cv::goodFeaturesToTrack(forw_img, n_pts, n_max_cnt, 0.01, param.min_dist, mask);\n                \n            }\n            else\n                n_pts.clear();\n            \n            addPoints();\n        }\n        \n        prev_img = cur_img;\n        prev_pts = cur_pts;\n        prev_un_pts = cur_un_pts;\n        \n        cur_img = forw_img;\n        cur_pts = forw_pts;\n        \n        undistortedPoints();\n        \n        prev_time = cur_time;\n    }\n    \n    void MonoTracker::addPoints()\n    {\n        for (auto& p: n_pts)\n        {\n            forw_pts.push_back(p);\n            ids.push_back(-1);\n            track_cnt.push_back(1);\n        }\n    }   \n    \n    void MonoTracker::undistortedPoints()\n    {\n        cur_un_pts.clear();\n        cur_un_pts_map.clear();\n        \n        for (unsigned int i = 0; i < cur_pts.size(); ++i)\n        {\n            Eigen::Vector2d a(cur_pts[i].x, cur_pts[i].y);\n            Eigen::Vector3d b;\n            m_camera->liftProjective(a, b);\n            cur_un_pts.push_back(cv::Point2f(b.x()/b.z(), b.y()/b.z()));\n            cur_un_pts_map.insert(std::make_pair(ids[i], cv::Point2f(b.x()/b.z(), b.y()/b.z())));\n        }\n        \n        if (!prev_un_pts_map.empty())\n        {\n            double dt = cur_time - prev_time;\n            pts_velocity.clear();\n            \n            for (unsigned int i = 0; i < cur_un_pts.size(); ++i)\n            {\n                if (ids[i] != -1)\n                {\n                    auto it = prev_un_pts_map.find(ids[i]);\n                    if (it != prev_un_pts_map.end())\n                    {\n                        double v_x = (cur_un_pts[i].x - it->second.x) / dt;\n                        double v_y = (cur_un_pts[i].y - it->second.y) / dt;\n                        pts_velocity.push_back(cv::Point2f(v_x, v_y));\n                    }\n                    else\n                        pts_velocity.push_back(cv::Point2f(0.0, 0.0));\n                }\n                else\n                    pts_velocity.push_back(cv::Point2f(0.0, 0.0));\n            }\n        }\n        else\n        {\n            for (unsigned int i = 0; i < cur_pts.size(); ++i)\n                pts_velocity.push_back(cv::Point2f(0.0, 0.0));\n        }\n        \n        prev_un_pts_map = cur_un_pts_map;\n    }\n    \n    void MonoTracker::setMask()\n    {\n        mask = cv::Mat(param.row, param.col, CV_8UC1, cv::Scalar(255));\n        \n        std::vector<std::pair<int, std::pair<cv::Point2f, int>>> cnt_pts_id;\n        \n        for (unsigned int i = 0; i < forw_pts.size(); ++i)\n            cnt_pts_id.push_back(std::make_pair(track_cnt[i], std::make_pair(forw_pts[i], ids[i])));\n        \n        std::sort(cnt_pts_id.begin(), cnt_pts_id.end(), [](const std::pair<int, std::pair<cv::Point2f, int>>& a, const std::pair<int, std::pair<cv::Point2f, int>>& b){ return a.first > b.first; });\n        \n        forw_pts.clear();\n        ids.clear();\n        track_cnt.clear();\n        \n        for (auto& it: cnt_pts_id)\n        {\n            if (mask.at<uchar>(it.second.first) == 255)\n            {\n                forw_pts.push_back(it.second.first);\n                ids.push_back(it.second.second);\n                track_cnt.push_back(it.first);\n                \n                cv::circle(mask, it.second.first, param.min_dist, 0, -1);\n            }\n        }\n\n    }\n    \n    \n    void MonoTracker::rejectWithF()\n    {\n        if (forw_pts.size() >= 8)\n        {\n            std::vector<cv::Point2f> un_cur_pts(cur_pts.size()), un_forw_pts(forw_pts.size());\n            \n            for (unsigned int i = 0; i < cur_pts.size(); ++i)\n            {\n                Eigen::Vector3d tmp_p;\n                m_camera->liftProjective(Eigen::Vector2d(cur_pts[i].x, cur_pts[i].y), tmp_p);\n                \n                tmp_p.x() = param.focal_length*tmp_p.x()/tmp_p.z() + param.col/2.0;\n                tmp_p.y() = param.focal_length*tmp_p.y()/tmp_p.z() + param.row/2.0;\n                un_cur_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());\n                \n                m_camera->liftProjective(Eigen::Vector2d(forw_pts[i].x, forw_pts[i].y), tmp_p);\n                tmp_p.x() = param.focal_length*tmp_p.x()/tmp_p.z() + param.col/2.0;\n                tmp_p.y() = param.focal_length*tmp_p.y()/tmp_p.z() + param.row/2.0;\n                un_forw_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());\n            }\n            \n            std::vector<uchar> status;\n            cv::findFundamentalMat(un_cur_pts, un_forw_pts, cv::FM_RANSAC, param.f_threshold, 0.99, status);\n            \n            // int size_a = cur_pts.size();\n            reduceVector<cv::Point2f>(prev_pts, status);\n            reduceVector<cv::Point2f>(cur_pts, status);\n            reduceVector<cv::Point2f>(forw_pts, status);\n            reduceVector<cv::Point2f>(cur_un_pts, status);\n            reduceVector<int>(ids, status);\n            reduceVector<int>(track_cnt, status);\n            \n            // ROS_INFO(\"FM RANSAC: %d points reduced to -> %lu: %f\", size_a, forw_pts.size(), 1.0*forw_pts.size()/size_a);\n        }\n    }\n    \n    \n    bool MonoTracker::inBorder(const cv::Point2f& pt)\n    {\n        const int BORDER_SIZE = 1;\n        int img_x = cvRound(pt.x);\n        int img_y = cvRound(pt.y);\n        \n        return BORDER_SIZE <= img_x && img_x < param.col-BORDER_SIZE && BORDER_SIZE <= img_y && img_y < param.row-BORDER_SIZE;\n    }\n    \n    void MonoTracker::readIntrinsicParameter(const std::string& calib_file)\n    {\n        ROS_INFO(\"[Mono Tracker]: Reading parameter of camera from: %s\", calib_file.c_str());\n        \n        m_camera = camera_model::CameraFactory::instance()->generateCameraFromYamlFile(calib_file);\n    }\n    \n}\n"
  },
  {
    "path": "feature_tracker/src/mono_tracker_node.cpp",
    "content": "/**   This File is part of Feature Tracker\n *  \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n *    \n *    A few functions of the mono tracker mimic the realization from GVINS\n *    <https://github.com/HKUST-Aerial-Robotics/GVINS>\n *    GVINS is under GPLv3 License.\n */\n\n#include \"mono_tracker.h\"\n    \nint main(int argc, char** argv)\n{\n    ros::init(argc, argv, \"mono_tracker\");\n    ros::NodeHandle n(\"~\");\n    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);\n    \n    feature_tracker::MonoTrackerPtr mono_tracker_ptr(new feature_tracker::MonoTracker(n));\n    \n    mono_tracker_ptr->init();\n        \n    ros::spin();\n    return 0;\n}\n    \n"
  },
  {
    "path": "feature_tracker/src/stereo_parameters.cpp",
    "content": "/**   This File is part of Feature Tracker\n *  \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#include \"stereo_parameters.h\"\n#include <opencv2/core/eigen.hpp>\n\nnamespace feature_tracker\n{\n    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())\n    {}\n    \n    void StereoParameters::readParameters(ros::NodeHandle& n)\n    {\n        std::string config_file;\n        config_file = readParam<std::string>(n, \"config_file_left\");\n        cam_left_path = config_file;\n        config_file = readParam<std::string>(n, \"config_file_right\");\n        cam_right_path = config_file;\n        \n        cv::FileStorage fs_left(cam_left_path, cv::FileStorage::READ);\n        if (!fs_left.isOpened())\n        {\n            ROS_ERROR(\"ERROR: Wrong path to stereo left config yaml files!\");\n            n.shutdown();\n        }\n        \n        row = fs_left[\"image_height\"];\n        col = fs_left[\"image_width\"];\n        fs_left[\"cam_left_topic\"] >> image_left_topic;\n        fs_left[\"cam_right_topic\"] >> image_right_topic;\n        fs_left[\"camera_name\"] >> cam_left_name;\n        max_cnt = fs_left[\"max_cnt\"];\n        min_dist = fs_left[\"min_dist\"];\n        window_size = fs_left[\"window_size\"];\n        freq = fs_left[\"freq\"];\n        \n        if (freq == 0) freq = 100;\n        \n        show_track = fs_left[\"show_track\"];\n        \n        show_timer = fs_left[\"show_timer\"];\n        timer_warning_thres = fs_left[\"timer_warning_thres\"];\n        \n        equalize = fs_left[\"equalize\"];\n        \n        epipolar_thres = static_cast<double>(fs_left[\"epipolar_thres\"]);\n        F_threshold = static_cast<double>(fs_left[\"F_threshold\"]);\n        \n        print_track_info = fs_left[\"print_track_info\"];\n        \n        cv::Mat cv_R1, cv_T1;\n        fs_left[\"extrinsicRotation\"] >> cv_R1;\n        fs_left[\"extrinsicTranslation\"] >> cv_T1;\n        Eigen::Matrix3d eigen_R1;\n        Eigen::Vector3d eigen_T1;\n        cv::cv2eigen(cv_R1, eigen_R1);\n        cv::cv2eigen(cv_T1, eigen_T1);\n        T_cl2i.linear() = eigen_R1;\n        T_cl2i.translation() = eigen_T1;\n        \n        fs_left.release();\n        \n        cv::FileStorage fs_right(cam_right_path, cv::FileStorage::READ);\n        if (!fs_right.isOpened())\n        {\n            ROS_ERROR(\"ERROR: Wrong path to stereo right config yaml files!\");\n            n.shutdown();            \n        }\n        \n        fs_right[\"camera_name\"] >> cam_right_name;\n        \n        cv::Mat cv_R2, cv_T2;\n        fs_right[\"extrinsicRotation\"] >> cv_R2;\n        fs_right[\"extrinsicTranslation\"] >> cv_T2;\n        Eigen::Matrix3d eigen_R2;\n        Eigen::Vector3d eigen_T2;\n        cv::cv2eigen(cv_R2, eigen_R2);\n        cv::cv2eigen(cv_T2, eigen_T2);\n        T_cr2i.linear() = eigen_R2;\n        T_cr2i.translation() = eigen_T2;\n        \n        fs_right.release();\n    }\n    \n}\n"
  },
  {
    "path": "feature_tracker/src/stereo_tracker.cpp",
    "content": "/**   This File is part of Feature Tracker\n *  \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n \n#include \"stereo_tracker.h\"\n\n#include \"Color.h\"\n\nnamespace feature_tracker\n{\n    unsigned long long int StereoTracker::n_id = 0;\n    \n    void StereoTracker::init()\n    {\n        param.readParameters(nh);\n        \n        readIntrinsicParameters();\n        \n        cam_left_sub_.reset(new message_filters::Subscriber<sensor_msgs::Image>(nh, param.image_left_topic, 50));\n        cam_right_sub_.reset(new message_filters::Subscriber<sensor_msgs::Image>(nh, param.image_right_topic, 50));\n        stereo_sub_.reset(new message_filters::Synchronizer<stereoSyncPolicy>(stereoSyncPolicy(50), *cam_left_sub_, *cam_right_sub_));\n        stereo_sub_->registerCallback(boost::bind(&StereoTracker::stereo_callback, this, _1, _2));\n        \n        pub_stereo_feature = nh.advertise<feature_tracker::StereoFrame>(\"stereo_feature\", 50);\n        pub_match = nh.advertise<sensor_msgs::Image>(\"stereo_track\", 50);\n        \n        first_image_flag = true;\n        first_image_time = 0.0;\n        last_image_time = 0.0;\n        \n        pub_this_frame = false;\n        pub_count = 1;\n    }\n    \n    void StereoTracker::readIntrinsicParameters()\n    {\n        ROS_INFO(\"[Stereo Tracker]: Reading parameter of left camera from: %s\", param.cam_left_path.c_str());\n        \n        m_camera_left = camera_model::CameraFactory::instance()->generateCameraFromYamlFile(param.cam_left_path);\n        \n        ROS_INFO(\"[Stereo Tracker]: Reading parameter of right camera from: %s\", param.cam_right_path.c_str());\n        \n        m_camera_right = camera_model::CameraFactory::instance()->generateCameraFromYamlFile(param.cam_right_path);        \n    }\n    \n    void StereoTracker::stereo_callback(const sensor_msgs::Image::ConstPtr& cam_left_msg, const sensor_msgs::Image::ConstPtr& cam_right_msg)\n    {\n        TicToc stereo_timer;\n        \n        if (first_image_flag)\n        {\n            first_image_flag = false;\n            first_image_time = cam_left_msg->header.stamp.toSec();\n            last_image_time = cam_left_msg->header.stamp.toSec();\n            return;\n        }\n        \n        if (cam_left_msg->header.stamp.toSec() - last_image_time > 1.0 || cam_left_msg->header.stamp.toSec() < last_image_time)\n        {\n            ROS_WARN(\"[Stereo Tracker]: Image discontinue! Stereo tracker reset!\");\n            first_image_flag = true;\n            first_image_time = 0.0;\n            last_image_time = 0.0;\n            pub_count = 1;\n            pub_this_frame = false;\n            return;\n        }\n        \n        last_image_time = cam_left_msg->header.stamp.toSec();\n        \n        if (std::round(1.0*pub_count/(last_image_time-first_image_time)) <= param.freq)\n        {\n            pub_this_frame = true;\n            \n            if (std::fabs(1.0*pub_count/(last_image_time-first_image_time)-param.freq) < 0.01*param.freq)\n            {\n                first_image_time = last_image_time;\n                pub_count = 0;\n            }\n        }\n        else\n        {\n            pub_this_frame = false;\n        }        \n        \n        cv_bridge::CvImageConstPtr left_ptr, right_ptr;\n        if (cam_left_msg->encoding == \"8UC1\")\n        {\n            sensor_msgs::Image img;\n            img.header = cam_left_msg->header;\n            img.height = cam_left_msg->height;\n            img.width = cam_left_msg->width;\n            img.is_bigendian = cam_left_msg->is_bigendian;\n            img.step = cam_left_msg->step;\n            img.data = cam_left_msg->data;\n            img.encoding = \"mono8\";\n            left_ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8);\n        }\n        else\n            left_ptr = cv_bridge::toCvCopy(cam_left_msg, sensor_msgs::image_encodings::MONO8);\n        if (cam_right_msg->encoding == \"8UC1\")\n        {\n            sensor_msgs::Image img;\n            img.header = cam_right_msg->header;\n            img.height = cam_right_msg->height;\n            img.width = cam_right_msg->width;\n            img.is_bigendian = cam_right_msg->is_bigendian;\n            img.step = cam_right_msg->step;\n            img.data = cam_right_msg->data;\n            img.encoding = \"mono8\";\n            right_ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8);\n        }\n        else\n            right_ptr = cv_bridge::toCvCopy(cam_right_msg, sensor_msgs::image_encodings::MONO8);\n        \n        processStereoImage(left_ptr->image, right_ptr->image, cam_left_msg->header.stamp.toSec());\n        \n        if (pub_this_frame)\n        {\n            ++pub_count;\n            \n            feature_tracker::StereoFrame::Ptr stereo_frame_ptr(new feature_tracker::StereoFrame);\n            stereo_frame_ptr->header = cam_left_msg->header;\n            \n            for (unsigned int i = 0; i < ids.size(); ++i)\n            {\n                if (track_cnt[i] > 1)\n                {\n                    feature_tracker::StereoMeas::Ptr stereo_feature_ptr(new feature_tracker::StereoMeas);\n                    \n                    stereo_feature_ptr->id = ids[i];\n                    stereo_feature_ptr->u0 = cur_left_un_pts[i].x;\n                    stereo_feature_ptr->v0 = cur_left_un_pts[i].y;\n                    stereo_feature_ptr->u1 = cur_right_un_pts[i].x;\n                    stereo_feature_ptr->v1 = cur_right_un_pts[i].y;\n                    \n                    stereo_frame_ptr->stereo_features.push_back(*stereo_feature_ptr);\n                }\n            }\n            \n            if (stereo_frame_ptr->stereo_features.size() > 0)\n                pub_stereo_feature.publish(stereo_frame_ptr);\n            \n            if (param.show_track)\n            {\n                cv::Mat track_img(param.row, 2*param.col, CV_8UC3);\n                cv::cvtColor(left_ptr->image, track_img.colRange(0, param.col), CV_GRAY2BGR);\n                cv::cvtColor(right_ptr->image, track_img.colRange(param.col, 2*param.col), CV_GRAY2BGR);\n                \n                for (unsigned int j = 0; j < cur_left_pts.size(); ++j)\n                {\n                    double len = std::min(1.0, 1.0*track_cnt[j]/param.window_size);\n                    \n                    cv::circle(track_img, cur_left_pts[j], 3, cv::Scalar(255*(1-len), 0, 255*len), 2);\n                    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);\n                    \n                    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);\n                }\n                \n                cv_bridge::CvImage track_img_ros(cam_left_msg->header, \"bgr8\", track_img);\n                \n                pub_match.publish(track_img_ros.toImageMsg());\n            }\n        }\n        \n        if (param.show_timer)\n        {\n            double stereo_time = stereo_timer.toc();\n            \n            if (stereo_time <= param.timer_warning_thres)\n                std::cout << color::setBlue << \"[StereoTracker]: Stereo tracking processing time = \" << stereo_time << \" (ms) \" << color::resetColor << std::endl;\n            else\n                std::cout << color::setRed << \"[StereoTracker]: Stereo tracking processing time = \" << stereo_time << \" (ms) \" << color::resetColor << std::endl;\n        }\n    }\n    \n    void StereoTracker::processStereoImage(const cv::Mat& _left_img, const cv::Mat& _right_img, double _cur_time)\n    {\n        cv::Mat left_img, right_img;\n        cur_time = _cur_time;\n        \n        if (param.equalize)\n        {\n            cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));\n            clahe->apply(_left_img, left_img);\n            clahe->apply(_right_img, right_img);\n        }\n        else\n        {\n            left_img = _left_img;\n            right_img = _right_img;\n        }\n        \n        if (forw_left_img.empty())\n        {\n            prev_left_img = cur_left_img = forw_left_img = left_img;\n            prev_right_img = cur_right_img = forw_right_img = right_img;\n        }\n        else\n        {\n            forw_left_img = left_img;\n            forw_right_img = right_img;\n        }\n        \n        forw_left_pts.clear();\n        forw_right_pts.clear();\n        \n        if (cur_left_pts.size() > 0)\n        {\n            std::vector<uchar> status;\n            std::vector<float> err;\n            \n            cv::calcOpticalFlowPyrLK(cur_left_img, forw_left_img, cur_left_pts, forw_left_pts, status, err, cv::Size(21, 21), 3);\n            \n            for (unsigned int i = 0; i < forw_left_pts.size(); ++i)\n                if (status[i] && !inBorder(forw_left_pts[i]))\n                    status[i] = 0;\n                \n            reduceTwinVector<cv::Point2f>(prev_left_pts, prev_right_pts, status);\n            reduceTwinVector<cv::Point2f>(cur_left_pts, cur_right_pts, status);\n            reduceVector<cv::Point2f>(forw_left_pts, status);\n            reduceVector<unsigned long long int>(ids, status);\n            // reduceTwinVector<cv::Point2f>(prev_left_un_pts, prev_right_un_pts, status);\n            reduceTwinVector<cv::Point2f>(cur_left_un_pts, cur_right_un_pts, status);\n            reduceVector<unsigned int>(track_cnt, status);\n            \n            if (forw_left_pts.size() > 0)\n            {\n                status.clear();\n                err.clear();\n                \n                predictHomography(forw_left_pts, forw_right_pts);\n            \n                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);\n            \n                for (unsigned int i = 0; i < forw_right_pts.size(); ++i)\n                    if (status[i] && !inBorder(forw_right_pts[i]))\n                        status[i] = 0;\n                \n                markStereoMatchOutlier(forw_left_pts, forw_right_pts, status);\n            \n                reduceTwinVector<cv::Point2f>(prev_left_pts, prev_right_pts, status);\n                reduceTwinVector<cv::Point2f>(cur_left_pts, cur_right_pts, status);\n                reduceTwinVector<cv::Point2f>(forw_left_pts, forw_right_pts, status);\n                reduceVector<unsigned long long int>(ids, status);\n                // reduceTwinVector<cv::Point2f>(prev_left_un_pts, prev_right_un_pts, status);\n                reduceTwinVector<cv::Point2f>(cur_left_un_pts, cur_right_un_pts, status);\n                reduceVector<unsigned int>(track_cnt, status);\n            }    \n        }\n        \n        for (auto& n: track_cnt)\n            ++n;\n        \n        if (pub_this_frame)\n        {\n            rejectWithTwinRANSAC();\n            \n            setMask();\n            \n            int n_max_cnt = param.max_cnt - static_cast<int>(forw_left_pts.size());\n            \n            if (n_max_cnt > 0)\n            {\n                if (mask.empty())\n                    ROS_WARN(\"[Stereo Tracker]: Mask is empty!\");\n                if (mask.type() != CV_8UC1)\n                    ROS_WARN(\"[Stereo Tracker]: Mask type wrong!\");\n                if (mask.size() != forw_left_img.size())\n                    ROS_WARN(\"[Stereo Tracker]: Mask size wrong!\");\n\n                n_left_pts.clear();\n                n_right_pts.clear();\n                \n                cv::goodFeaturesToTrack(forw_left_img, n_left_pts, n_max_cnt, 0.01, param.min_dist, mask);\n                \n                \n                if (n_left_pts.size() > 0)\n                {\n                    std::vector<uchar> status;\n                    std::vector<float> err;\n                    \n                    predictHomography(n_left_pts, n_right_pts);\n                    \n                    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);\n                    \n\n                    for (unsigned int i = 0; i < n_right_pts.size(); ++i)\n                        if (status[i] && !inBorder(n_right_pts[i]))\n                            status[i] = 0;\n                    \n                    markStereoMatchOutlier(n_left_pts, n_right_pts, status);\n                \n                    reduceTwinVector<cv::Point2f>(n_left_pts, n_right_pts, status);\n                }\n                \n            }\n            else\n            {\n                n_left_pts.clear();\n                n_right_pts.clear();\n            }\n            \n            addPoints();\n        }\n        \n        prev_left_img = cur_left_img;\n        prev_right_img = cur_right_img;\n        \n        prev_left_pts = cur_left_pts;\n        prev_right_pts = cur_right_pts;\n        \n        prev_left_un_pts = cur_left_un_pts;\n        prev_right_un_pts = cur_right_un_pts;\n        \n        cur_left_img = forw_left_img;\n        cur_right_img = forw_right_img;\n        \n        cur_left_pts = forw_left_pts;\n        cur_right_pts = forw_right_pts;\n        \n        undistortedPoints();\n        \n        prev_time = cur_time;\n        \n        updateID();\n        \n        if (param.print_track_info)\n            ROS_INFO(\"[Stereo Tracker]: Current feature pairs: %i, including newly detected pairs: %i\", int(cur_left_pts.size()), int(n_left_pts.size()));\n    }\n    \n    void StereoTracker::predictHomography(const std::vector<cv::Point2f>& left_pts, std::vector<cv::Point2f>& right_pts)\n    {\n        right_pts.clear();\n        const Eigen::Matrix3d C_cl2cr = param.T_cr2i.linear().transpose()*param.T_cl2i.linear();\n        \n        for (unsigned int i = 0; i < left_pts.size(); ++i)\n        {\n            Eigen::Vector3d tmp_lpt;\n            m_camera_left->liftProjective(Eigen::Vector2d(left_pts[i].x, left_pts[i].y), tmp_lpt);\n            \n            Eigen::Vector3d tmp_rpt = C_cl2cr*tmp_lpt;\n            Eigen::Vector2d rpt;\n            m_camera_right->spaceToPlane(tmp_rpt, rpt);\n            right_pts.push_back(cv::Point2f(rpt.x(), rpt.y()));\n        }    \n    }\n    \n    void StereoTracker::publishStereoImg(const cv::Mat& left_img, const cv::Mat& right_img, const std::vector<cv::Point2f>& left_pts, const std::vector<cv::Point2f>& right_pts, const int row, const int col, bool pair_line)\n    {\n        cv::Mat track_img(row, 2*col, CV_8UC3);\n        cv::cvtColor(left_img, track_img.colRange(0, col), CV_GRAY2BGR);\n        cv::cvtColor(right_img, track_img.colRange(col, 2*col), CV_GRAY2BGR);\n                \n        for (unsigned int j = 0; j < left_pts.size(); ++j)\n            cv::circle(track_img, left_pts[j], 3, cv::Scalar(255, 0, 0), 2);\n        \n        for (unsigned int j = 0; j < right_pts.size(); ++j)\n            cv::circle(track_img, cv::Point2f(right_pts[j].x + col, right_pts[j].y), 3, cv::Scalar(255, 0, 0), 2);\n        \n        if (pair_line && left_pts.size() == right_pts.size())\n            for (unsigned int j = 0; j < left_pts.size(); ++j)\n                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); \n            \n        cv::imshow(\"stereo_match\", track_img);\n        cv::waitKey(0);\n    }\n    \n    void StereoTracker::updateID()\n    {\n        for (unsigned int i = 0; i < ids.size(); ++i)\n            if (ids[i] == -1)\n                ids[i] = n_id++;\n    }\n    \n    void StereoTracker::undistortedPoints()\n    {\n        cur_left_un_pts.clear();\n        cur_right_un_pts.clear();\n        \n        for (unsigned int i = 0; i < cur_left_pts.size(); ++i)\n        {\n            Eigen::Vector2d al(cur_left_pts[i].x, cur_left_pts[i].y);\n            Eigen::Vector2d ar(cur_right_pts[i].x, cur_right_pts[i].y);\n            \n            Eigen::Vector3d bl, br;\n            \n            m_camera_left->liftProjective(al, bl);\n            m_camera_right->liftProjective(ar, br);\n            \n            cur_left_un_pts.push_back(cv::Point2f(bl.x()/bl.z(), bl.y()/bl.z()));\n            cur_right_un_pts.push_back(cv::Point2f(br.x()/br.z(), br.y()/br.z()));\n        }\n        \n        \n    }\n    \n    void StereoTracker::addPoints()\n    {\n        for (unsigned int i = 0; i < n_left_pts.size(); ++i)\n        {\n            forw_left_pts.push_back(n_left_pts[i]);\n            forw_right_pts.push_back(n_right_pts[i]);\n            ids.push_back(-1);\n            track_cnt.push_back(1);\n        }\n    }\n    \n    void StereoTracker::setMask()\n    {\n        using namespace std;\n        \n        mask = cv::Mat(param.row, param.col, CV_8UC1, cv::Scalar(255));\n        \n        typedef pair<unsigned int, pair<cv::Point2f, pair<cv::Point2f, unsigned long long int>>> pts_pair;\n        typedef vector<pts_pair> pts_pair_vector;\n        \n        pts_pair_vector cnt_lpts_rpts_id;\n        \n        for (unsigned int i = 0; i < forw_left_pts.size(); ++i)\n            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]))));\n        \n        sort(cnt_lpts_rpts_id.begin(), cnt_lpts_rpts_id.end(), [](const pts_pair& a, const pts_pair& b){ return a.first > b.first; });\n        \n        forw_left_pts.clear();\n        forw_right_pts.clear();\n        ids.clear();\n        track_cnt.clear();\n        \n        for (auto& it: cnt_lpts_rpts_id)\n        {\n            if (mask.at<uchar>(it.second.first) == 255)\n            {\n                forw_left_pts.push_back(it.second.first);\n                forw_right_pts.push_back(it.second.second.first);\n                track_cnt.push_back(it.first);\n                ids.push_back(it.second.second.second);\n                \n                cv::circle(mask, it.second.first, param.min_dist, 0, -1);\n            }\n        }\n    }\n    \n    void StereoTracker::rejectWithTwinRANSAC()\n    {\n        int origin_pts_num = forw_left_pts.size();\n        \n        if (forw_left_pts.size() >= 8)\n        {\n            std::vector<cv::Point2f> un_cur_left_pts(cur_left_pts.size()), un_forw_left_pts(forw_left_pts.size());\n            \n            double left_focal = 1.0/m_camera_left->getNormalPixel();\n            \n            for (unsigned int i = 0; i < cur_left_pts.size(); ++i)\n            {\n                Eigen::Vector3d tmp_p;\n                m_camera_left->liftProjective(Eigen::Vector2d(cur_left_pts[i].x, cur_left_pts[i].y), tmp_p);\n                \n                tmp_p.x() = left_focal*tmp_p.x()/tmp_p.z() + param.col/2.0;\n                tmp_p.y() = left_focal*tmp_p.y()/tmp_p.z() + param.row/2.0;\n                un_cur_left_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());\n                \n                m_camera_left->liftProjective(Eigen::Vector2d(forw_left_pts[i].x, forw_left_pts[i].y), tmp_p);\n                tmp_p.x() = left_focal*tmp_p.x()/tmp_p.z() + param.col/2.0;\n                tmp_p.y() = left_focal*tmp_p.y()/tmp_p.z() + param.row/2.0;\n                un_forw_left_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());\n            }\n            \n            std::vector<uchar> status;\n            cv::findFundamentalMat(un_cur_left_pts, un_forw_left_pts, cv::FM_RANSAC, param.F_threshold, 0.99, status);\n            \n            reduceTwinVector<cv::Point2f>(prev_left_pts, prev_right_pts, status);\n            reduceTwinVector<cv::Point2f>(cur_left_pts, cur_right_pts, status);\n            reduceTwinVector<cv::Point2f>(forw_left_pts, forw_right_pts, status);\n            reduceTwinVector<cv::Point2f>(cur_left_un_pts, cur_right_un_pts, status);\n            // reduceTwinVector<cv::Point2f>(prev_left_un_pts, prev_right_un_pts, status);\n            reduceVector<unsigned long long int>(ids, status);\n            reduceVector<unsigned int>(track_cnt, status);\n        }\n        \n        if (forw_right_pts.size() > 8)\n        {\n            std::vector<cv::Point2f> un_cur_right_pts(cur_right_pts.size()), un_forw_right_pts(forw_right_pts.size());\n            \n            double right_focal = 1.0/m_camera_right->getNormalPixel();\n            \n            for (unsigned int i = 0; i < cur_right_pts.size(); ++i)\n            {\n                Eigen::Vector3d tmp_p;\n                m_camera_right->liftProjective(Eigen::Vector2d(cur_right_pts[i].x, cur_right_pts[i].y), tmp_p);\n                \n                tmp_p.x() = right_focal*tmp_p.x()/tmp_p.z() + param.col/2.0;\n                tmp_p.y() = right_focal*tmp_p.y()/tmp_p.z() + param.row/2.0;\n                un_cur_right_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());\n                \n                m_camera_right->liftProjective(Eigen::Vector2d(forw_right_pts[i].x, forw_right_pts[i].y), tmp_p);\n                tmp_p.x() = right_focal*tmp_p.x()/tmp_p.z() + param.col/2.0;\n                tmp_p.y() = right_focal*tmp_p.y()/tmp_p.z() + param.row/2.0;\n                un_forw_right_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());\n            }\n            \n            std::vector<uchar> status;\n            cv::findFundamentalMat(un_cur_right_pts, un_forw_right_pts, cv::FM_RANSAC, param.F_threshold, 0.99, status);\n            \n            reduceTwinVector<cv::Point2f>(prev_left_pts, prev_right_pts, status);\n            reduceTwinVector<cv::Point2f>(cur_left_pts, cur_right_pts, status);\n            reduceTwinVector<cv::Point2f>(forw_left_pts, forw_right_pts, status);\n            reduceTwinVector<cv::Point2f>(cur_left_un_pts, cur_right_un_pts, status); // reduceTwinVector<cv::Point2f>(prev_left_un_pts, prev_right_un_pts, status);\n            reduceVector<unsigned long long int>(ids, status);\n            reduceVector<unsigned int>(track_cnt, status);            \n        }\n        \n        int final_pts_num = forw_left_pts.size();\n        \n        if (origin_pts_num > 0 && final_pts_num != origin_pts_num && param.print_track_info)\n            ROS_INFO(\"[Stereo Tracker]: Twin side RANSAC remove %f %% of points.\", 100.0-100.0*float(final_pts_num)/float(origin_pts_num));\n    }\n    \n    void StereoTracker::markStereoMatchOutlier(const std::vector<cv::Point2f>& left_pts, const std::vector<cv::Point2f>& right_pts, std::vector<uchar>& status)\n    {\n        Eigen::Isometry3d T_cl2cr = param.T_cr2i.inverse()*param.T_cl2i;\n        \n        Eigen::Matrix3d t_hat;\n        const Eigen::Vector3d& t = T_cl2cr.translation();\n        t_hat <<    0.0,   -t.z(),   t.y(),\n                  t.z(),      0.0,  -t.x(),\n                 -t.y(),    t.x(),     0.0;\n        \n        const Eigen::Matrix3d essen_mat = t_hat*T_cl2cr.linear();\n        \n        double unmatch_stereo_thres = 2.0*param.epipolar_thres/(1.0/m_camera_left->getNormalPixel() + 1.0/m_camera_right->getNormalPixel());\n        \n        int remove_num = 0;\n        int total_num = 0;\n        \n        for (unsigned int i = 0; i < left_pts.size(); ++i)\n        {\n            if (status[i] == 0) continue;\n            \n            ++total_num;\n            \n            Eigen::Vector3d left_proj, right_proj;\n            m_camera_left->liftProjective(Eigen::Vector2d(left_pts[i].x, left_pts[i].y), left_proj);\n            m_camera_right->liftProjective(Eigen::Vector2d(right_pts[i].x, right_pts[i].y), right_proj);\n            \n            Eigen::Vector3d epipolar_line = essen_mat*left_proj;\n            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]));\n            \n            if (norm_err > unmatch_stereo_thres)\n            {\n                status[i] = 0;\n                ++remove_num;\n            }\n        }\n        \n        if (total_num > 0 && param.print_track_info)\n            ROS_INFO(\"[Stereo Tracker]: Remove %f %% of stereo matches through epipolar constrains!\", float(remove_num)/float(total_num)*100.0);\n        \n    }\n    \n    bool StereoTracker::inBorder(const cv::Point2f& pt)\n    {\n        const int BORDER_SIZE = 1;\n        int img_x = cvRound(pt.x);\n        int img_y = cvRound(pt.y);\n        \n        return BORDER_SIZE <= img_x && img_x < param.col-BORDER_SIZE && BORDER_SIZE <= img_y && img_y < param.row-BORDER_SIZE;     \n    }\n}\n"
  },
  {
    "path": "feature_tracker/src/stereo_tracker_node.cpp",
    "content": "/**   This File is part of Feature Tracker\n *  \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#include \"stereo_tracker.h\"\n\nint main(int argc, char** argv)\n{\n    ros::init(argc, argv, \"stereo_tracker\");\n    ros::NodeHandle n(\"~\");\n    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);\n    \n    feature_tracker::StereoTrackerPtr stereo_tracker_ptr(new feature_tracker::StereoTracker(n));\n    \n    stereo_tracker_ptr->init();\n    \n    ros::spin();\n    return 0;\n}\n"
  },
  {
    "path": "gnss_comm/.gitignore",
    "content": ".vscode/"
  },
  {
    "path": "gnss_comm/CMakeLists.txt",
    "content": "cmake_minimum_required(VERSION 2.8.3)\nproject(gnss_comm)\n\nset(CMAKE_BUILD_TYPE \"release\")\nset(CMAKE_CXX_FLAGS \"-std=c++11 \")\nset(CMAKE_CXX_FLAGS_RELEASE \"-O3 -Wall -fPIC\")\n\nfind_package(catkin REQUIRED COMPONENTS\n  roscpp\n  rospy\n  std_msgs\n  message_generation\n)\n\nfile(GLOB_RECURSE message_files ${PROJECT_SOURCE_DIR}/msg/*.msg)\nadd_message_files(FILES \n    GnssTimeMsg.msg\n    GnssEphemMsg.msg\n    GnssGloEphemMsg.msg\n    GnssMeasMsg.msg\n    GnssObsMsg.msg\n    GnssBestXYZMsg.msg\n    GnssPVTSolnMsg.msg\n    GnssSvsMsg.msg\n    GnssTimePulseInfoMsg.msg\n    StampedFloat64Array.msg\n)\n\ngenerate_messages(\n  DEPENDENCIES std_msgs\n)\n\ncatkin_package(\n  CATKIN_DEPENDS message_runtime\n)\n\ncatkin_package(\n    INCLUDE_DIRS include\n    LIBRARIES ${PROJECT_NAME}\n    CATKIN_DEPENDS roscpp std_msgs message_generation\n    DEPENDS\n)\n\nset(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)\nfind_package(Eigen3 REQUIRED)\nfind_package(Glog REQUIRED)\ninclude_directories(\n  ${EIGEN3_INCLUDE_DIR}\n  ${GLOG_INCLUDE_DIR}\n  ${catkin_INCLUDE_DIRS}\n  ${PROJECT_SOURCE_DIR}/include/\n)\n\nfile(GLOB_RECURSE source_files ${PROJECT_SOURCE_DIR}/src/*.cpp)\nadd_library(${PROJECT_NAME} ${source_files})\ntarget_include_directories(${PROJECT_NAME} PUBLIC\n  ${EIGEN3_INCLUDE_DIR}\n  ${catkin_INCLUDE_DIRS}\n  ${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/\n)\ntarget_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${GLOG_LIBRARIES})\nadd_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages_cpp)\n"
  },
  {
    "path": "gnss_comm/LICENSE",
    "content": "                    GNU GENERAL PUBLIC LICENSE\n                       Version 3, 29 June 2007\n\n Copyright (C) 2007 Free Software Foundation, Inc. <https://fsf.org/>\n Everyone is permitted to copy and distribute verbatim copies\n of this license document, but changing it is not allowed.\n\n                            Preamble\n\n  The GNU General Public License is a free, copyleft license for\nsoftware and other kinds of works.\n\n  The licenses for most software and other practical works are designed\nto take away your freedom to share and change the works.  By contrast,\nthe GNU General Public License is intended to guarantee your freedom to\nshare and change all versions of a program--to make sure it remains free\nsoftware for all its users.  We, the Free Software Foundation, use the\nGNU General Public License for most of our software; it applies also to\nany other work released this way by its authors.  You can apply it to\nyour programs, too.\n\n  When we speak of free software, we are referring to freedom, not\nprice.  Our General Public Licenses are designed to make sure that you\nhave the freedom to distribute copies of free software (and charge for\nthem if you wish), that you receive source code or can get it if you\nwant it, that you can change the software or use pieces of it in new\nfree programs, and that you know you can do these things.\n\n  To protect your rights, we need to prevent others from denying you\nthese rights or asking you to surrender the rights.  Therefore, you have\ncertain responsibilities if you distribute copies of the software, or if\nyou modify it: responsibilities to respect the freedom of others.\n\n  For example, if you distribute copies of such a program, whether\ngratis or for a fee, you must pass on to the recipients the same\nfreedoms that you received.  You must make sure that they, too, receive\nor can get the source code.  And you must show them these terms so they\nknow their rights.\n\n  Developers that use the GNU GPL protect your rights with two steps:\n(1) assert copyright on the software, and (2) offer you this License\ngiving you legal permission to copy, distribute and/or modify it.\n\n  For the developers' and authors' protection, the GPL clearly explains\nthat there is no warranty for this free software.  For both users' and\nauthors' sake, the GPL requires that modified versions be marked as\nchanged, so that their problems will not be attributed erroneously to\nauthors of previous versions.\n\n  Some devices are designed to deny users access to install or run\nmodified versions of the software inside them, although the manufacturer\ncan do so.  This is fundamentally incompatible with the aim of\nprotecting users' freedom to change the software.  The systematic\npattern of such abuse occurs in the area of products for individuals to\nuse, which is precisely where it is most unacceptable.  Therefore, we\nhave designed this version of the GPL to prohibit the practice for those\nproducts.  If such problems arise substantially in other domains, we\nstand ready to extend this provision to those domains in future versions\nof the GPL, as needed to protect the freedom of users.\n\n  Finally, every program is threatened constantly by software patents.\nStates should not allow patents to restrict development and use of\nsoftware on general-purpose computers, but in those that do, we wish to\navoid the special danger that patents applied to a free program could\nmake it effectively proprietary.  To prevent this, the GPL assures that\npatents cannot be used to render the program non-free.\n\n  The precise terms and conditions for copying, distribution and\nmodification follow.\n\n                       TERMS AND CONDITIONS\n\n  0. Definitions.\n\n  \"This License\" refers to version 3 of the GNU General Public License.\n\n  \"Copyright\" also means copyright-like laws that apply to other kinds of\nworks, such as semiconductor masks.\n\n  \"The Program\" refers to any copyrightable work licensed under this\nLicense.  Each licensee is addressed as \"you\".  \"Licensees\" and\n\"recipients\" may be individuals or organizations.\n\n  To \"modify\" a work means to copy from or adapt all or part of the work\nin a fashion requiring copyright permission, other than the making of an\nexact copy.  The resulting work is called a \"modified version\" of the\nearlier work or a work \"based on\" the earlier work.\n\n  A \"covered work\" means either the unmodified Program or a work based\non the Program.\n\n  To \"propagate\" a work means to do anything with it that, without\npermission, would make you directly or secondarily liable for\ninfringement under applicable copyright law, except executing it on a\ncomputer or modifying a private copy.  Propagation includes copying,\ndistribution (with or without modification), making available to the\npublic, and in some countries other activities as well.\n\n  To \"convey\" a work means any kind of propagation that enables other\nparties to make or receive copies.  Mere interaction with a user through\na computer network, with no transfer of a copy, is not conveying.\n\n  An interactive user interface displays \"Appropriate Legal Notices\"\nto the extent that it includes a convenient and prominently visible\nfeature that (1) displays an appropriate copyright notice, and (2)\ntells the user that there is no warranty for the work (except to the\nextent that warranties are provided), that licensees may convey the\nwork under this License, and how to view a copy of this License.  If\nthe interface presents a list of user commands or options, such as a\nmenu, a prominent item in the list meets this criterion.\n\n  1. Source Code.\n\n  The \"source code\" for a work means the preferred form of the work\nfor making modifications to it.  \"Object code\" means any non-source\nform of a work.\n\n  A \"Standard Interface\" means an interface that either is an official\nstandard defined by a recognized standards body, or, in the case of\ninterfaces specified for a particular programming language, one that\nis widely used among developers working in that language.\n\n  The \"System Libraries\" of an executable work include anything, other\nthan the work as a whole, that (a) is included in the normal form of\npackaging a Major Component, but which is not part of that Major\nComponent, and (b) serves only to enable use of the work with that\nMajor Component, or to implement a Standard Interface for which an\nimplementation is available to the public in source code form.  A\n\"Major Component\", in this context, means a major essential component\n(kernel, window system, and so on) of the specific operating system\n(if any) on which the executable work runs, or a compiler used to\nproduce the work, or an object code interpreter used to run it.\n\n  The \"Corresponding Source\" for a work in object code form means all\nthe source code needed to generate, install, and (for an executable\nwork) run the object code and to modify the work, including scripts to\ncontrol those activities.  However, it does not include the work's\nSystem Libraries, or general-purpose tools or generally available free\nprograms which are used unmodified in performing those activities but\nwhich are not part of the work.  For example, Corresponding Source\nincludes interface definition files associated with source files for\nthe work, and the source code for shared libraries and dynamically\nlinked subprograms that the work is specifically designed to require,\nsuch as by intimate data communication or control flow between those\nsubprograms and other parts of the work.\n\n  The Corresponding Source need not include anything that users\ncan regenerate automatically from other parts of the Corresponding\nSource.\n\n  The Corresponding Source for a work in source code form is that\nsame work.\n\n  2. Basic Permissions.\n\n  All rights granted under this License are granted for the term of\ncopyright on the Program, and are irrevocable provided the stated\nconditions are met.  This License explicitly affirms your unlimited\npermission to run the unmodified Program.  The output from running a\ncovered work is covered by this License only if the output, given its\ncontent, constitutes a covered work.  This License acknowledges your\nrights of fair use or other equivalent, as provided by copyright law.\n\n  You may make, run and propagate covered works that you do not\nconvey, without conditions so long as your license otherwise remains\nin force.  You may convey covered works to others for the sole purpose\nof having them make modifications exclusively for you, or provide you\nwith facilities for running those works, provided that you comply with\nthe terms of this License in conveying all material for which you do\nnot control copyright.  Those thus making or running the covered works\nfor you must do so exclusively on your behalf, under your direction\nand control, on terms that prohibit them from making any copies of\nyour copyrighted material outside their relationship with you.\n\n  Conveying under any other circumstances is permitted solely under\nthe conditions stated below.  Sublicensing is not allowed; section 10\nmakes it unnecessary.\n\n  3. Protecting Users' Legal Rights From Anti-Circumvention Law.\n\n  No covered work shall be deemed part of an effective technological\nmeasure under any applicable law fulfilling obligations under article\n11 of the WIPO copyright treaty adopted on 20 December 1996, or\nsimilar laws prohibiting or restricting circumvention of such\nmeasures.\n\n  When you convey a covered work, you waive any legal power to forbid\ncircumvention of technological measures to the extent such circumvention\nis effected by exercising rights under this License with respect to\nthe covered work, and you disclaim any intention to limit operation or\nmodification of the work as a means of enforcing, against the work's\nusers, your or third parties' legal rights to forbid circumvention of\ntechnological measures.\n\n  4. Conveying Verbatim Copies.\n\n  You may convey verbatim copies of the Program's source code as you\nreceive it, in any medium, provided that you conspicuously and\nappropriately publish on each copy an appropriate copyright notice;\nkeep intact all notices stating that this License and any\nnon-permissive terms added in accord with section 7 apply to the code;\nkeep intact all notices of the absence of any warranty; and give all\nrecipients a copy of this License along with the Program.\n\n  You may charge any price or no price for each copy that you convey,\nand you may offer support or warranty protection for a fee.\n\n  5. Conveying Modified Source Versions.\n\n  You may convey a work based on the Program, or the modifications to\nproduce it from the Program, in the form of source code under the\nterms of section 4, provided that you also meet all of these conditions:\n\n    a) The work must carry prominent notices stating that you modified\n    it, and giving a relevant date.\n\n    b) The work must carry prominent notices stating that it is\n    released under this License and any conditions added under section\n    7.  This requirement modifies the requirement in section 4 to\n    \"keep intact all notices\".\n\n    c) You must license the entire work, as a whole, under this\n    License to anyone who comes into possession of a copy.  This\n    License will therefore apply, along with any applicable section 7\n    additional terms, to the whole of the work, and all its parts,\n    regardless of how they are packaged.  This License gives no\n    permission to license the work in any other way, but it does not\n    invalidate such permission if you have separately received it.\n\n    d) If the work has interactive user interfaces, each must display\n    Appropriate Legal Notices; however, if the Program has interactive\n    interfaces that do not display Appropriate Legal Notices, your\n    work need not make them do so.\n\n  A compilation of a covered work with other separate and independent\nworks, which are not by their nature extensions of the covered work,\nand which are not combined with it such as to form a larger program,\nin or on a volume of a storage or distribution medium, is called an\n\"aggregate\" if the compilation and its resulting copyright are not\nused to limit the access or legal rights of the compilation's users\nbeyond what the individual works permit.  Inclusion of a covered work\nin an aggregate does not cause this License to apply to the other\nparts of the aggregate.\n\n  6. Conveying Non-Source Forms.\n\n  You may convey a covered work in object code form under the terms\nof sections 4 and 5, provided that you also convey the\nmachine-readable Corresponding Source under the terms of this License,\nin one of these ways:\n\n    a) Convey the object code in, or embodied in, a physical product\n    (including a physical distribution medium), accompanied by the\n    Corresponding Source fixed on a durable physical medium\n    customarily used for software interchange.\n\n    b) Convey the object code in, or embodied in, a physical product\n    (including a physical distribution medium), accompanied by a\n    written offer, valid for at least three years and valid for as\n    long as you offer spare parts or customer support for that product\n    model, to give anyone who possesses the object code either (1) a\n    copy of the Corresponding Source for all the software in the\n    product that is covered by this License, on a durable physical\n    medium customarily used for software interchange, for a price no\n    more than your reasonable cost of physically performing this\n    conveying of source, or (2) access to copy the\n    Corresponding Source from a network server at no charge.\n\n    c) Convey individual copies of the object code with a copy of the\n    written offer to provide the Corresponding Source.  This\n    alternative is allowed only occasionally and noncommercially, and\n    only if you received the object code with such an offer, in accord\n    with subsection 6b.\n\n    d) Convey the object code by offering access from a designated\n    place (gratis or for a charge), and offer equivalent access to the\n    Corresponding Source in the same way through the same place at no\n    further charge.  You need not require recipients to copy the\n    Corresponding Source along with the object code.  If the place to\n    copy the object code is a network server, the Corresponding Source\n    may be on a different server (operated by you or a third party)\n    that supports equivalent copying facilities, provided you maintain\n    clear directions next to the object code saying where to find the\n    Corresponding Source.  Regardless of what server hosts the\n    Corresponding Source, you remain obligated to ensure that it is\n    available for as long as needed to satisfy these requirements.\n\n    e) Convey the object code using peer-to-peer transmission, provided\n    you inform other peers where the object code and Corresponding\n    Source of the work are being offered to the general public at no\n    charge under subsection 6d.\n\n  A separable portion of the object code, whose source code is excluded\nfrom the Corresponding Source as a System Library, need not be\nincluded in conveying the object code work.\n\n  A \"User Product\" is either (1) a \"consumer product\", which means any\ntangible personal property which is normally used for personal, family,\nor household purposes, or (2) anything designed or sold for incorporation\ninto a dwelling.  In determining whether a product is a consumer product,\ndoubtful cases shall be resolved in favor of coverage.  For a particular\nproduct received by a particular user, \"normally used\" refers to a\ntypical or common use of that class of product, regardless of the status\nof the particular user or of the way in which the particular user\nactually uses, or expects or is expected to use, the product.  A product\nis a consumer product regardless of whether the product has substantial\ncommercial, industrial or non-consumer uses, unless such uses represent\nthe only significant mode of use of the product.\n\n  \"Installation Information\" for a User Product means any methods,\nprocedures, authorization keys, or other information required to install\nand execute modified versions of a covered work in that User Product from\na modified version of its Corresponding Source.  The information must\nsuffice to ensure that the continued functioning of the modified object\ncode is in no case prevented or interfered with solely because\nmodification has been made.\n\n  If you convey an object code work under this section in, or with, or\nspecifically for use in, a User Product, and the conveying occurs as\npart of a transaction in which the right of possession and use of the\nUser Product is transferred to the recipient in perpetuity or for a\nfixed term (regardless of how the transaction is characterized), the\nCorresponding Source conveyed under this section must be accompanied\nby the Installation Information.  But this requirement does not apply\nif neither you nor any third party retains the ability to install\nmodified object code on the User Product (for example, the work has\nbeen installed in ROM).\n\n  The requirement to provide Installation Information does not include a\nrequirement to continue to provide support service, warranty, or updates\nfor a work that has been modified or installed by the recipient, or for\nthe User Product in which it has been modified or installed.  Access to a\nnetwork may be denied when the modification itself materially and\nadversely affects the operation of the network or violates the rules and\nprotocols for communication across the network.\n\n  Corresponding Source conveyed, and Installation Information provided,\nin accord with this section must be in a format that is publicly\ndocumented (and with an implementation available to the public in\nsource code form), and must require no special password or key for\nunpacking, reading or copying.\n\n  7. Additional Terms.\n\n  \"Additional permissions\" are terms that supplement the terms of this\nLicense by making exceptions from one or more of its conditions.\nAdditional permissions that are applicable to the entire Program shall\nbe treated as though they were included in this License, to the extent\nthat they are valid under applicable law.  If additional permissions\napply only to part of the Program, that part may be used separately\nunder those permissions, but the entire Program remains governed by\nthis License without regard to the additional permissions.\n\n  When you convey a copy of a covered work, you may at your option\nremove any additional permissions from that copy, or from any part of\nit.  (Additional permissions may be written to require their own\nremoval in certain cases when you modify the work.)  You may place\nadditional permissions on material, added by you to a covered work,\nfor which you have or can give appropriate copyright permission.\n\n  Notwithstanding any other provision of this License, for material you\nadd to a covered work, you may (if authorized by the copyright holders of\nthat material) supplement the terms of this License with terms:\n\n    a) Disclaiming warranty or limiting liability differently from the\n    terms of sections 15 and 16 of this License; or\n\n    b) Requiring preservation of specified reasonable legal notices or\n    author attributions in that material or in the Appropriate Legal\n    Notices displayed by works containing it; or\n\n    c) Prohibiting misrepresentation of the origin of that material, or\n    requiring that modified versions of such material be marked in\n    reasonable ways as different from the original version; or\n\n    d) Limiting the use for publicity purposes of names of licensors or\n    authors of the material; or\n\n    e) Declining to grant rights under trademark law for use of some\n    trade names, trademarks, or service marks; or\n\n    f) Requiring indemnification of licensors and authors of that\n    material by anyone who conveys the material (or modified versions of\n    it) with contractual assumptions of liability to the recipient, for\n    any liability that these contractual assumptions directly impose on\n    those licensors and authors.\n\n  All other non-permissive additional terms are considered \"further\nrestrictions\" within the meaning of section 10.  If the Program as you\nreceived it, or any part of it, contains a notice stating that it is\ngoverned by this License along with a term that is a further\nrestriction, you may remove that term.  If a license document contains\na further restriction but permits relicensing or conveying under this\nLicense, you may add to a covered work material governed by the terms\nof that license document, provided that the further restriction does\nnot survive such relicensing or conveying.\n\n  If you add terms to a covered work in accord with this section, you\nmust place, in the relevant source files, a statement of the\nadditional terms that apply to those files, or a notice indicating\nwhere to find the applicable terms.\n\n  Additional terms, permissive or non-permissive, may be stated in the\nform of a separately written license, or stated as exceptions;\nthe above requirements apply either way.\n\n  8. Termination.\n\n  You may not propagate or modify a covered work except as expressly\nprovided under this License.  Any attempt otherwise to propagate or\nmodify it is void, and will automatically terminate your rights under\nthis License (including any patent licenses granted under the third\nparagraph of section 11).\n\n  However, if you cease all violation of this License, then your\nlicense from a particular copyright holder is reinstated (a)\nprovisionally, unless and until the copyright holder explicitly and\nfinally terminates your license, and (b) permanently, if the copyright\nholder fails to notify you of the violation by some reasonable means\nprior to 60 days after the cessation.\n\n  Moreover, your license from a particular copyright holder is\nreinstated permanently if the copyright holder notifies you of the\nviolation by some reasonable means, this is the first time you have\nreceived notice of violation of this License (for any work) from that\ncopyright holder, and you cure the violation prior to 30 days after\nyour receipt of the notice.\n\n  Termination of your rights under this section does not terminate the\nlicenses of parties who have received copies or rights from you under\nthis License.  If your rights have been terminated and not permanently\nreinstated, you do not qualify to receive new licenses for the same\nmaterial under section 10.\n\n  9. Acceptance Not Required for Having Copies.\n\n  You are not required to accept this License in order to receive or\nrun a copy of the Program.  Ancillary propagation of a covered work\noccurring solely as a consequence of using peer-to-peer transmission\nto receive a copy likewise does not require acceptance.  However,\nnothing other than this License grants you permission to propagate or\nmodify any covered work.  These actions infringe copyright if you do\nnot accept this License.  Therefore, by modifying or propagating a\ncovered work, you indicate your acceptance of this License to do so.\n\n  10. Automatic Licensing of Downstream Recipients.\n\n  Each time you convey a covered work, the recipient automatically\nreceives a license from the original licensors, to run, modify and\npropagate that work, subject to this License.  You are not responsible\nfor enforcing compliance by third parties with this License.\n\n  An \"entity transaction\" is a transaction transferring control of an\norganization, or substantially all assets of one, or subdividing an\norganization, or merging organizations.  If propagation of a covered\nwork results from an entity transaction, each party to that\ntransaction who receives a copy of the work also receives whatever\nlicenses to the work the party's predecessor in interest had or could\ngive under the previous paragraph, plus a right to possession of the\nCorresponding Source of the work from the predecessor in interest, if\nthe predecessor has it or can get it with reasonable efforts.\n\n  You may not impose any further restrictions on the exercise of the\nrights granted or affirmed under this License.  For example, you may\nnot impose a license fee, royalty, or other charge for exercise of\nrights granted under this License, and you may not initiate litigation\n(including a cross-claim or counterclaim in a lawsuit) alleging that\nany patent claim is infringed by making, using, selling, offering for\nsale, or importing the Program or any portion of it.\n\n  11. Patents.\n\n  A \"contributor\" is a copyright holder who authorizes use under this\nLicense of the Program or a work on which the Program is based.  The\nwork thus licensed is called the contributor's \"contributor version\".\n\n  A contributor's \"essential patent claims\" are all patent claims\nowned or controlled by the contributor, whether already acquired or\nhereafter acquired, that would be infringed by some manner, permitted\nby this License, of making, using, or selling its contributor version,\nbut do not include claims that would be infringed only as a\nconsequence of further modification of the contributor version.  For\npurposes of this definition, \"control\" includes the right to grant\npatent sublicenses in a manner consistent with the requirements of\nthis License.\n\n  Each contributor grants you a non-exclusive, worldwide, royalty-free\npatent license under the contributor's essential patent claims, to\nmake, use, sell, offer for sale, import and otherwise run, modify and\npropagate the contents of its contributor version.\n\n  In the following three paragraphs, a \"patent license\" is any express\nagreement or commitment, however denominated, not to enforce a patent\n(such as an express permission to practice a patent or covenant not to\nsue for patent infringement).  To \"grant\" such a patent license to a\nparty means to make such an agreement or commitment not to enforce a\npatent against the party.\n\n  If you convey a covered work, knowingly relying on a patent license,\nand the Corresponding Source of the work is not available for anyone\nto copy, free of charge and under the terms of this License, through a\npublicly available network server or other readily accessible means,\nthen you must either (1) cause the Corresponding Source to be so\navailable, or (2) arrange to deprive yourself of the benefit of the\npatent license for this particular work, or (3) arrange, in a manner\nconsistent with the requirements of this License, to extend the patent\nlicense to downstream recipients.  \"Knowingly relying\" means you have\nactual knowledge that, but for the patent license, your conveying the\ncovered work in a country, or your recipient's use of the covered work\nin a country, would infringe one or more identifiable patents in that\ncountry that you have reason to believe are valid.\n\n  If, pursuant to or in connection with a single transaction or\narrangement, you convey, or propagate by procuring conveyance of, a\ncovered work, and grant a patent license to some of the parties\nreceiving the covered work authorizing them to use, propagate, modify\nor convey a specific copy of the covered work, then the patent license\nyou grant is automatically extended to all recipients of the covered\nwork and works based on it.\n\n  A patent license is \"discriminatory\" if it does not include within\nthe scope of its coverage, prohibits the exercise of, or is\nconditioned on the non-exercise of one or more of the rights that are\nspecifically granted under this License.  You may not convey a covered\nwork if you are a party to an arrangement with a third party that is\nin the business of distributing software, under which you make payment\nto the third party based on the extent of your activity of conveying\nthe work, and under which the third party grants, to any of the\nparties who would receive the covered work from you, a discriminatory\npatent license (a) in connection with copies of the covered work\nconveyed by you (or copies made from those copies), or (b) primarily\nfor and in connection with specific products or compilations that\ncontain the covered work, unless you entered into that arrangement,\nor that patent license was granted, prior to 28 March 2007.\n\n  Nothing in this License shall be construed as excluding or limiting\nany implied license or other defenses to infringement that may\notherwise be available to you under applicable patent law.\n\n  12. No Surrender of Others' Freedom.\n\n  If conditions are imposed on you (whether by court order, agreement or\notherwise) that contradict the conditions of this License, they do not\nexcuse you from the conditions of this License.  If you cannot convey a\ncovered work so as to satisfy simultaneously your obligations under this\nLicense and any other pertinent obligations, then as a consequence you may\nnot convey it at all.  For example, if you agree to terms that obligate you\nto collect a royalty for further conveying from those to whom you convey\nthe Program, the only way you could satisfy both those terms and this\nLicense would be to refrain entirely from conveying the Program.\n\n  13. Use with the GNU Affero General Public License.\n\n  Notwithstanding any other provision of this License, you have\npermission to link or combine any covered work with a work licensed\nunder version 3 of the GNU Affero General Public License into a single\ncombined work, and to convey the resulting work.  The terms of this\nLicense will continue to apply to the part which is the covered work,\nbut the special requirements of the GNU Affero General Public License,\nsection 13, concerning interaction through a network will apply to the\ncombination as such.\n\n  14. Revised Versions of this License.\n\n  The Free Software Foundation may publish revised and/or new versions of\nthe GNU General Public License from time to time.  Such new versions will\nbe similar in spirit to the present version, but may differ in detail to\naddress new problems or concerns.\n\n  Each version is given a distinguishing version number.  If the\nProgram specifies that a certain numbered version of the GNU General\nPublic License \"or any later version\" applies to it, you have the\noption of following the terms and conditions either of that numbered\nversion or of any later version published by the Free Software\nFoundation.  If the Program does not specify a version number of the\nGNU General Public License, you may choose any version ever published\nby the Free Software Foundation.\n\n  If the Program specifies that a proxy can decide which future\nversions of the GNU General Public License can be used, that proxy's\npublic statement of acceptance of a version permanently authorizes you\nto choose that version for the Program.\n\n  Later license versions may give you additional or different\npermissions.  However, no additional obligations are imposed on any\nauthor or copyright holder as a result of your choosing to follow a\nlater version.\n\n  15. Disclaimer of Warranty.\n\n  THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY\nAPPLICABLE LAW.  EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT\nHOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM \"AS IS\" WITHOUT WARRANTY\nOF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO,\nTHE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR\nPURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM\nIS WITH YOU.  SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF\nALL NECESSARY SERVICING, REPAIR OR CORRECTION.\n\n  16. Limitation of Liability.\n\n  IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING\nWILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS\nTHE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY\nGENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE\nUSE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF\nDATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD\nPARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS),\nEVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF\nSUCH DAMAGES.\n\n  17. Interpretation of Sections 15 and 16.\n\n  If the disclaimer of warranty and limitation of liability provided\nabove cannot be given local legal effect according to their terms,\nreviewing courts shall apply local law that most closely approximates\nan absolute waiver of all civil liability in connection with the\nProgram, unless a warranty or assumption of liability accompanies a\ncopy of the Program in return for a fee.\n\n                     END OF TERMS AND CONDITIONS\n\n            How to Apply These Terms to Your New Programs\n\n  If you develop a new program, and you want it to be of the greatest\npossible use to the public, the best way to achieve this is to make it\nfree software which everyone can redistribute and change under these terms.\n\n  To do so, attach the following notices to the program.  It is safest\nto attach them to the start of each source file to most effectively\nstate the exclusion of warranty; and each file should have at least\nthe \"copyright\" line and a pointer to where the full notice is found.\n\n    <one line to give the program's name and a brief idea of what it does.>\n    Copyright (C) <year>  <name of author>\n\n    This program is free software: you can redistribute it and/or modify\n    it under the terms of the GNU General Public License as published by\n    the Free Software Foundation, either version 3 of the License, or\n    (at your option) any later version.\n\n    This program is distributed in the hope that it will be useful,\n    but WITHOUT ANY WARRANTY; without even the implied warranty of\n    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n    GNU General Public License for more details.\n\n    You should have received a copy of the GNU General Public License\n    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n\nAlso add information on how to contact you by electronic and paper mail.\n\n  If the program does terminal interaction, make it output a short\nnotice like this when it starts in an interactive mode:\n\n    <program>  Copyright (C) <year>  <name of author>\n    This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'.\n    This is free software, and you are welcome to redistribute it\n    under certain conditions; type `show c' for details.\n\nThe hypothetical commands `show w' and `show c' should show the appropriate\nparts of the General Public License.  Of course, your program's commands\nmight be different; for a GUI interface, you would use an \"about box\".\n\n  You should also get your employer (if you work as a programmer) or school,\nif any, to sign a \"copyright disclaimer\" for the program, if necessary.\nFor more information on this, and how to apply and follow the GNU GPL, see\n<https://www.gnu.org/licenses/>.\n\n  The GNU General Public License does not permit incorporating your program\ninto proprietary programs.  If your program is a subroutine library, you\nmay consider it more useful to permit linking proprietary applications with\nthe library.  If this is what you want to do, use the GNU Lesser General\nPublic License instead of this License.  But first, please read\n<https://www.gnu.org/licenses/why-not-lgpl.html>.\n"
  },
  {
    "path": "gnss_comm/README.md",
    "content": "# gnss_comm\n\n**Authors/Maintainers:** CAO Shaozu (shaozu.cao AT gmail.com)\n\nThe *gnss_comm* package contains basic definitions and utility functions for GNSS raw measurement processing. \n\n## 1. Prerequisites\n\n### 1.1 C++11 Compiler\nThis package requires some features of C++11.\n\n### 1.2 ROS\nThis package is developed under [ROS Kinetic](http://wiki.ros.org/kinetic) environment.\n\n### 1.3 Eigen\nOur 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:\n\n```\ncd eigen-3.3.3/\nmkdir build\ncd build\ncmake ..\nsudo make install\n```\n\n### 1.4 Glog\nWe use google's glog library for message output. If you are using Ubuntu, install it by:\n```\nsudo apt-get install libgoogle-glog-dev\n```\nIf 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.\n\n\n## 2. Build gnss_comm library\nClone the repository to your catkin workspace (for example `~/catkin_ws/`):\n```\ncd ~/catkin_ws/src/\ngit clone https://github.com/HKUST-Aerial-Robotics/gnss_comm.git\n```\nThen build the package with:\n```\ncd ~/catkin_ws/\ncatkin_make\nsource ~/catkin_ws/devel/setup.bash\n```\nIf you encounter any problem during the building of *gnss_comm*, try with docker in [the next section](#docker_section).\n\n## 3. <a name=\"docker_section\"></a>Docker Support\nTo 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:\n```\ncd ~/catkin_ws/src/gnss_comm/docker\nmake build\n```\nThe 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.\n\n## 4. Acknowledgements\nMany of the definitions and utility functions in this package are adapted from [RTKLIB](http://www.rtklib.com/).\n\n## 5. License\nThe source code is released under [GPLv3](https://www.gnu.org/licenses/gpl-3.0.html) license.\n"
  },
  {
    "path": "gnss_comm/cmake/FindEigen.cmake",
    "content": "# Ceres Solver - A fast non-linear least squares minimizer\n# Copyright 2015 Google Inc. All rights reserved.\n# http://ceres-solver.org/\n#\n# Redistribution and use in source and binary forms, with or without\n# modification, are permitted provided that the following conditions are met:\n#\n# * Redistributions of source code must retain the above copyright notice,\n#   this list of conditions and the following disclaimer.\n# * Redistributions in binary form must reproduce the above copyright notice,\n#   this list of conditions and the following disclaimer in the documentation\n#   and/or other materials provided with the distribution.\n# * Neither the name of Google Inc. nor the names of its contributors may be\n#   used to endorse or promote products derived from this software without\n#   specific prior written permission.\n#\n# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"\n# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\n# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE\n# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE\n# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR\n# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF\n# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS\n# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN\n# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)\n# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n# POSSIBILITY OF SUCH DAMAGE.\n#\n# Author: alexs.mac@gmail.com (Alex Stewart)\n#\n\n# FindEigen.cmake - Find Eigen library, version >= 3.\n#\n# This module defines the following variables:\n#\n# EIGEN_FOUND: TRUE iff Eigen is found.\n# EIGEN_INCLUDE_DIRS: Include directories for Eigen.\n#\n# EIGEN_VERSION: Extracted from Eigen/src/Core/util/Macros.h\n# EIGEN_WORLD_VERSION: Equal to 3 if EIGEN_VERSION = 3.2.0\n# EIGEN_MAJOR_VERSION: Equal to 2 if EIGEN_VERSION = 3.2.0\n# EIGEN_MINOR_VERSION: Equal to 0 if EIGEN_VERSION = 3.2.0\n#\n# The following variables control the behaviour of this module:\n#\n# EIGEN_INCLUDE_DIR_HINTS: List of additional directories in which to\n#                          search for eigen includes, e.g: /timbuktu/eigen3.\n#\n# The following variables are also defined by this module, but in line with\n# CMake recommended FindPackage() module style should NOT be referenced directly\n# by callers (use the plural variables detailed above instead).  These variables\n# do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which\n# are NOT re-called (i.e. search for library is not repeated) if these variables\n# are set with valid values _in the CMake cache_. This means that if these\n# variables are set directly in the cache, either by the user in the CMake GUI,\n# or by the user passing -DVAR=VALUE directives to CMake when called (which\n# explicitly defines a cache variable), then they will be used verbatim,\n# bypassing the HINTS variables and other hard-coded search locations.\n#\n# EIGEN_INCLUDE_DIR: Include directory for CXSparse, not including the\n#                    include directory of any dependencies.\n\n# Called if we failed to find Eigen or any of it's required dependencies,\n# unsets all public (designed to be used externally) variables and reports\n# error message at priority depending upon [REQUIRED/QUIET/<NONE>] argument.\nmacro(EIGEN_REPORT_NOT_FOUND REASON_MSG)\n  unset(EIGEN_FOUND)\n  unset(EIGEN_INCLUDE_DIRS)\n  # Make results of search visible in the CMake GUI if Eigen has not\n  # been found so that user does not have to toggle to advanced view.\n  mark_as_advanced(CLEAR EIGEN_INCLUDE_DIR)\n  # Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by FindPackage()\n  # use the camelcase library name, not uppercase.\n  if (Eigen_FIND_QUIETLY)\n    message(STATUS \"Failed to find Eigen - \" ${REASON_MSG} ${ARGN})\n  elseif (Eigen_FIND_REQUIRED)\n    message(FATAL_ERROR \"Failed to find Eigen - \" ${REASON_MSG} ${ARGN})\n  else()\n    # Neither QUIETLY nor REQUIRED, use no priority which emits a message\n    # but continues configuration and allows generation.\n    message(\"-- Failed to find Eigen - \" ${REASON_MSG} ${ARGN})\n  endif ()\n  return()\nendmacro(EIGEN_REPORT_NOT_FOUND)\n\n# Protect against any alternative find_package scripts for this library having\n# been called previously (in a client project) which set EIGEN_FOUND, but not\n# the other variables we require / set here which could cause the search logic\n# here to fail.\nunset(EIGEN_FOUND)\n\n# Search user-installed locations first, so that we prefer user installs\n# to system installs where both exist.\nlist(APPEND EIGEN_CHECK_INCLUDE_DIRS\n  /usr/local/include\n  /usr/local/homebrew/include # Mac OS X\n  /opt/local/var/macports/software # Mac OS X.\n  /opt/local/include\n  /usr/include)\n# Additional suffixes to try appending to each search path.\nlist(APPEND EIGEN_CHECK_PATH_SUFFIXES\n  eigen3 # Default root directory for Eigen.\n  Eigen/include/eigen3 # Windows (for C:/Program Files prefix) < 3.3\n  Eigen3/include/eigen3 ) # Windows (for C:/Program Files prefix) >= 3.3\n\n# Search supplied hint directories first if supplied.\nfind_path(EIGEN_INCLUDE_DIR\n  NAMES Eigen/Core\n  PATHS ${EIGEN_INCLUDE_DIR_HINTS}\n  ${EIGEN_CHECK_INCLUDE_DIRS}\n  PATH_SUFFIXES ${EIGEN_CHECK_PATH_SUFFIXES})\n\nif (NOT EIGEN_INCLUDE_DIR OR\n    NOT EXISTS ${EIGEN_INCLUDE_DIR})\n  eigen_report_not_found(\n    \"Could not find eigen3 include directory, set EIGEN_INCLUDE_DIR to \"\n    \"path to eigen3 include directory, e.g. /usr/local/include/eigen3.\")\nendif (NOT EIGEN_INCLUDE_DIR OR\n       NOT EXISTS ${EIGEN_INCLUDE_DIR})\n\n# Mark internally as found, then verify. EIGEN_REPORT_NOT_FOUND() unsets\n# if called.\nset(EIGEN_FOUND TRUE)\n\n# Extract Eigen version from Eigen/src/Core/util/Macros.h\nif (EIGEN_INCLUDE_DIR)\n  set(EIGEN_VERSION_FILE ${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h)\n  if (NOT EXISTS ${EIGEN_VERSION_FILE})\n    eigen_report_not_found(\n      \"Could not find file: ${EIGEN_VERSION_FILE} \"\n      \"containing version information in Eigen install located at: \"\n      \"${EIGEN_INCLUDE_DIR}.\")\n  else (NOT EXISTS ${EIGEN_VERSION_FILE})\n    file(READ ${EIGEN_VERSION_FILE} EIGEN_VERSION_FILE_CONTENTS)\n\n    string(REGEX MATCH \"#define EIGEN_WORLD_VERSION [0-9]+\"\n      EIGEN_WORLD_VERSION \"${EIGEN_VERSION_FILE_CONTENTS}\")\n    string(REGEX REPLACE \"#define EIGEN_WORLD_VERSION ([0-9]+)\" \"\\\\1\"\n      EIGEN_WORLD_VERSION \"${EIGEN_WORLD_VERSION}\")\n\n    string(REGEX MATCH \"#define EIGEN_MAJOR_VERSION [0-9]+\"\n      EIGEN_MAJOR_VERSION \"${EIGEN_VERSION_FILE_CONTENTS}\")\n    string(REGEX REPLACE \"#define EIGEN_MAJOR_VERSION ([0-9]+)\" \"\\\\1\"\n      EIGEN_MAJOR_VERSION \"${EIGEN_MAJOR_VERSION}\")\n\n    string(REGEX MATCH \"#define EIGEN_MINOR_VERSION [0-9]+\"\n      EIGEN_MINOR_VERSION \"${EIGEN_VERSION_FILE_CONTENTS}\")\n    string(REGEX REPLACE \"#define EIGEN_MINOR_VERSION ([0-9]+)\" \"\\\\1\"\n      EIGEN_MINOR_VERSION \"${EIGEN_MINOR_VERSION}\")\n\n    # This is on a single line s/t CMake does not interpret it as a list of\n    # elements and insert ';' separators which would result in 3.;2.;0 nonsense.\n    set(EIGEN_VERSION \"${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}\")\n  endif (NOT EXISTS ${EIGEN_VERSION_FILE})\nendif (EIGEN_INCLUDE_DIR)\n\n# Set standard CMake FindPackage variables if found.\nif (EIGEN_FOUND)\n  set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR})\nendif (EIGEN_FOUND)\n\n# Handle REQUIRED / QUIET optional arguments and version.\ninclude(FindPackageHandleStandardArgs)\nfind_package_handle_standard_args(Eigen\n  REQUIRED_VARS EIGEN_INCLUDE_DIRS\n  VERSION_VAR EIGEN_VERSION)\n\n# Only mark internal variables as advanced if we found Eigen, otherwise\n# leave it visible in the standard GUI for the user to set manually.\nif (EIGEN_FOUND)\n  mark_as_advanced(FORCE EIGEN_INCLUDE_DIR)\nendif (EIGEN_FOUND)\n"
  },
  {
    "path": "gnss_comm/cmake/FindGlog.cmake",
    "content": "# Ceres Solver - A fast non-linear least squares minimizer\n# Copyright 2015 Google Inc. All rights reserved.\n# http://ceres-solver.org/\n#\n# Redistribution and use in source and binary forms, with or without\n# modification, are permitted provided that the following conditions are met:\n#\n# * Redistributions of source code must retain the above copyright notice,\n#   this list of conditions and the following disclaimer.\n# * Redistributions in binary form must reproduce the above copyright notice,\n#   this list of conditions and the following disclaimer in the documentation\n#   and/or other materials provided with the distribution.\n# * Neither the name of Google Inc. nor the names of its contributors may be\n#   used to endorse or promote products derived from this software without\n#   specific prior written permission.\n#\n# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"\n# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\n# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE\n# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE\n# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR\n# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF\n# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS\n# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN\n# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)\n# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n# POSSIBILITY OF SUCH DAMAGE.\n#\n# Author: alexs.mac@gmail.com (Alex Stewart)\n#\n\n# FindGlog.cmake - Find Google glog logging library.\n#\n# This module defines the following variables:\n#\n# GLOG_FOUND: TRUE iff glog is found.\n# GLOG_INCLUDE_DIRS: Include directories for glog.\n# GLOG_LIBRARIES: Libraries required to link glog.\n# FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION: True iff the version of glog found\n#                                           was built & installed / exported\n#                                           as a CMake package.\n#\n# The following variables control the behaviour of this module:\n#\n# GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION: TRUE/FALSE, iff TRUE then\n#                           then prefer using an exported CMake configuration\n#                           generated by glog > 0.3.4 over searching for the\n#                           glog components manually.  Otherwise (FALSE)\n#                           ignore any exported glog CMake configurations and\n#                           always perform a manual search for the components.\n#                           Default: TRUE iff user does not define this variable\n#                           before we are called, and does NOT specify either\n#                           GLOG_INCLUDE_DIR_HINTS or GLOG_LIBRARY_DIR_HINTS\n#                           otherwise FALSE.\n# GLOG_INCLUDE_DIR_HINTS: List of additional directories in which to\n#                         search for glog includes, e.g: /timbuktu/include.\n# GLOG_LIBRARY_DIR_HINTS: List of additional directories in which to\n#                         search for glog libraries, e.g: /timbuktu/lib.\n#\n# The following variables are also defined by this module, but in line with\n# CMake recommended FindPackage() module style should NOT be referenced directly\n# by callers (use the plural variables detailed above instead).  These variables\n# do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which\n# are NOT re-called (i.e. search for library is not repeated) if these variables\n# are set with valid values _in the CMake cache_. This means that if these\n# variables are set directly in the cache, either by the user in the CMake GUI,\n# or by the user passing -DVAR=VALUE directives to CMake when called (which\n# explicitly defines a cache variable), then they will be used verbatim,\n# bypassing the HINTS variables and other hard-coded search locations.\n#\n# GLOG_INCLUDE_DIR: Include directory for glog, not including the\n#                   include directory of any dependencies.\n# GLOG_LIBRARY: glog library, not including the libraries of any\n#               dependencies.\n\n# Reset CALLERS_CMAKE_FIND_LIBRARY_PREFIXES to its value when\n# FindGlog was invoked.\nmacro(GLOG_RESET_FIND_LIBRARY_PREFIX)\n  if (MSVC AND CALLERS_CMAKE_FIND_LIBRARY_PREFIXES)\n    set(CMAKE_FIND_LIBRARY_PREFIXES \"${CALLERS_CMAKE_FIND_LIBRARY_PREFIXES}\")\n  endif()\nendmacro(GLOG_RESET_FIND_LIBRARY_PREFIX)\n\n# Called if we failed to find glog or any of it's required dependencies,\n# unsets all public (designed to be used externally) variables and reports\n# error message at priority depending upon [REQUIRED/QUIET/<NONE>] argument.\nmacro(GLOG_REPORT_NOT_FOUND REASON_MSG)\n  unset(GLOG_FOUND)\n  unset(GLOG_INCLUDE_DIRS)\n  unset(GLOG_LIBRARIES)\n  # Make results of search visible in the CMake GUI if glog has not\n  # been found so that user does not have to toggle to advanced view.\n  mark_as_advanced(CLEAR GLOG_INCLUDE_DIR\n                         GLOG_LIBRARY)\n\n  glog_reset_find_library_prefix()\n\n  # Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by FindPackage()\n  # use the camelcase library name, not uppercase.\n  if (Glog_FIND_QUIETLY)\n    message(STATUS \"Failed to find glog - \" ${REASON_MSG} ${ARGN})\n  elseif (Glog_FIND_REQUIRED)\n    message(FATAL_ERROR \"Failed to find glog - \" ${REASON_MSG} ${ARGN})\n  else()\n    # Neither QUIETLY nor REQUIRED, use no priority which emits a message\n    # but continues configuration and allows generation.\n    message(\"-- Failed to find glog - \" ${REASON_MSG} ${ARGN})\n  endif ()\n  return()\nendmacro(GLOG_REPORT_NOT_FOUND)\n\n# glog_message([mode] \"message text\")\n#\n# Wraps the standard cmake 'message' command, but suppresses output\n# if the QUIET flag was passed to the find_package(Glog ...) call.\nfunction(GLOG_MESSAGE)\n  if (NOT Glog_FIND_QUIETLY)\n    message(${ARGN})\n  endif()\nendfunction()\n\n# Protect against any alternative find_package scripts for this library having\n# been called previously (in a client project) which set GLOG_FOUND, but not\n# the other variables we require / set here which could cause the search logic\n# here to fail.\nunset(GLOG_FOUND)\n\n# -----------------------------------------------------------------\n# By default, if the user has expressed no preference for using an exported\n# glog CMake configuration over performing a search for the installed\n# components, and has not specified any hints for the search locations, then\n# prefer a glog exported configuration if available.\nif (NOT DEFINED GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION\n    AND NOT GLOG_INCLUDE_DIR_HINTS\n    AND NOT GLOG_LIBRARY_DIR_HINTS)\n  glog_message(STATUS \"No preference for use of exported glog CMake \"\n    \"configuration set, and no hints for include/library directories provided. \"\n    \"Defaulting to preferring an installed/exported glog CMake configuration \"\n    \"if available.\")\n  set(GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION TRUE)\nendif()\n\n# On macOS, add the Homebrew prefix (with appropriate suffixes) to the\n# respective HINTS directories (after any user-specified locations).  This\n# handles Homebrew installations into non-standard locations (not /usr/local).\n# We do not use CMAKE_PREFIX_PATH for this as given the search ordering of\n# find_xxx(), doing so would override any user-specified HINTS locations with\n# the Homebrew version if it exists.\nif (CMAKE_SYSTEM_NAME MATCHES \"Darwin\")\n  find_program(HOMEBREW_EXECUTABLE brew)\n  mark_as_advanced(FORCE HOMEBREW_EXECUTABLE)\n  if (HOMEBREW_EXECUTABLE)\n    # Detected a Homebrew install, query for its install prefix.\n    execute_process(COMMAND ${HOMEBREW_EXECUTABLE} --prefix\n      OUTPUT_VARIABLE HOMEBREW_INSTALL_PREFIX\n      OUTPUT_STRIP_TRAILING_WHITESPACE)\n    glog_message(STATUS \"Detected Homebrew with install prefix: \"\n      \"${HOMEBREW_INSTALL_PREFIX}, adding to CMake search paths.\")\n    list(APPEND GLOG_INCLUDE_DIR_HINTS \"${HOMEBREW_INSTALL_PREFIX}/include\")\n    list(APPEND GLOG_LIBRARY_DIR_HINTS \"${HOMEBREW_INSTALL_PREFIX}/lib\")\n  endif()\nendif()\n\nif (GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION)\n  # Try to find an exported CMake configuration for glog, as generated by\n  # glog versions > 0.3.4\n  #\n  # We search twice, s/t we can invert the ordering of precedence used by\n  # find_package() for exported package build directories, and installed\n  # packages (found via CMAKE_SYSTEM_PREFIX_PATH), listed as items 6) and 7)\n  # respectively in [1].\n  #\n  # By default, exported build directories are (in theory) detected first, and\n  # this is usually the case on Windows.  However, on OS X & Linux, the install\n  # path (/usr/local) is typically present in the PATH environment variable\n  # which is checked in item 4) in [1] (i.e. before both of the above, unless\n  # NO_SYSTEM_ENVIRONMENT_PATH is passed).  As such on those OSs installed\n  # packages are usually detected in preference to exported package build\n  # directories.\n  #\n  # To ensure a more consistent response across all OSs, and as users usually\n  # want to prefer an installed version of a package over a locally built one\n  # where both exist (esp. as the exported build directory might be removed\n  # after installation), we first search with NO_CMAKE_PACKAGE_REGISTRY which\n  # means any build directories exported by the user are ignored, and thus\n  # installed directories are preferred.  If this fails to find the package\n  # we then research again, but without NO_CMAKE_PACKAGE_REGISTRY, so any\n  # exported build directories will now be detected.\n  #\n  # To prevent confusion on Windows, we also pass NO_CMAKE_BUILDS_PATH (which\n  # is item 5) in [1]), to not preferentially use projects that were built\n  # recently with the CMake GUI to ensure that we always prefer an installed\n  # version if available.\n  #\n  # NOTE: We use the NAMES option as glog erroneously uses 'google-glog' as its\n  #       project name when built with CMake, but exports itself as just 'glog'.\n  #       On Linux/OS X this does not break detection as the project name is\n  #       not used as part of the install path for the CMake package files,\n  #       e.g. /usr/local/lib/cmake/glog, where the <glog> suffix is hardcoded\n  #       in glog's CMakeLists.  However, on Windows the project name *is*\n  #       part of the install prefix: C:/Program Files/google-glog/[include,lib].\n  #       However, by default CMake checks:\n  #       C:/Program Files/<FIND_PACKAGE_ARGUMENT_NAME='glog'> which does not\n  #       exist and thus detection fails.  Thus we use the NAMES to force the\n  #       search to use both google-glog & glog.\n  #\n  # [1] http://www.cmake.org/cmake/help/v2.8.11/cmake.html#command:find_package\n  find_package(glog QUIET\n                    NAMES google-glog glog\n                    HINTS ${glog_DIR} ${HOMEBREW_INSTALL_PREFIX}\n                    NO_MODULE\n                    NO_CMAKE_PACKAGE_REGISTRY\n                    NO_CMAKE_BUILDS_PATH)\n  if (glog_FOUND)\n    glog_message(STATUS \"Found installed version of glog: ${glog_DIR}\")\n  else()\n    # Failed to find an installed version of glog, repeat search allowing\n    # exported build directories.\n    glog_message(STATUS \"Failed to find installed glog CMake configuration, \"\n      \"searching for glog build directories exported with CMake.\")\n    # Again pass NO_CMAKE_BUILDS_PATH, as we know that glog is exported and\n    # do not want to treat projects built with the CMake GUI preferentially.\n    find_package(glog QUIET\n                      NAMES google-glog glog\n                      NO_MODULE\n                      NO_CMAKE_BUILDS_PATH)\n    if (glog_FOUND)\n      glog_message(STATUS \"Found exported glog build directory: ${glog_DIR}\")\n    endif(glog_FOUND)\n  endif(glog_FOUND)\n\n  set(FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION ${glog_FOUND})\n\n  if (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION)\n    glog_message(STATUS \"Detected glog version: ${glog_VERSION}\")\n    set(GLOG_FOUND ${glog_FOUND})\n    # glog wraps the include directories into the exported glog::glog target.\n    set(GLOG_INCLUDE_DIR \"\")\n    set(GLOG_LIBRARY glog::glog)\n  else (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION)\n    glog_message(STATUS \"Failed to find an installed/exported CMake \"\n      \"configuration for glog, will perform search for installed glog \"\n      \"components.\")\n  endif (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION)\nendif(GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION)\n\nif (NOT GLOG_FOUND)\n  # Either failed to find an exported glog CMake configuration, or user\n  # told us not to use one.  Perform a manual search for all glog components.\n\n  # Handle possible presence of lib prefix for libraries on MSVC, see\n  # also GLOG_RESET_FIND_LIBRARY_PREFIX().\n  if (MSVC)\n    # Preserve the caller's original values for CMAKE_FIND_LIBRARY_PREFIXES\n    # s/t we can set it back before returning.\n    set(CALLERS_CMAKE_FIND_LIBRARY_PREFIXES \"${CMAKE_FIND_LIBRARY_PREFIXES}\")\n    # The empty string in this list is important, it represents the case when\n    # the libraries have no prefix (shared libraries / DLLs).\n    set(CMAKE_FIND_LIBRARY_PREFIXES \"lib\" \"\" \"${CMAKE_FIND_LIBRARY_PREFIXES}\")\n  endif (MSVC)\n\n  # Search user-installed locations first, so that we prefer user installs\n  # to system installs where both exist.\n  list(APPEND GLOG_CHECK_INCLUDE_DIRS\n    /usr/local/include\n    /usr/local/homebrew/include # Mac OS X\n    /opt/local/var/macports/software # Mac OS X.\n    /opt/local/include\n    /usr/include)\n  # Windows (for C:/Program Files prefix).\n  list(APPEND GLOG_CHECK_PATH_SUFFIXES\n    glog/include\n    glog/Include\n    Glog/include\n    Glog/Include\n    google-glog/include # CMake installs with project name prefix.\n    google-glog/Include)\n\n  list(APPEND GLOG_CHECK_LIBRARY_DIRS\n    /usr/local/lib\n    /usr/local/homebrew/lib # Mac OS X.\n    /opt/local/lib\n    /usr/lib)\n  # Windows (for C:/Program Files prefix).\n  list(APPEND GLOG_CHECK_LIBRARY_SUFFIXES\n    glog/lib\n    glog/Lib\n    Glog/lib\n    Glog/Lib\n    google-glog/lib # CMake installs with project name prefix.\n    google-glog/Lib)\n\n  # Search supplied hint directories first if supplied.\n  find_path(GLOG_INCLUDE_DIR\n    NAMES glog/logging.h\n    HINTS ${GLOG_INCLUDE_DIR_HINTS}\n    PATHS ${GLOG_CHECK_INCLUDE_DIRS}\n    PATH_SUFFIXES ${GLOG_CHECK_PATH_SUFFIXES})\n  if (NOT GLOG_INCLUDE_DIR OR\n      NOT EXISTS ${GLOG_INCLUDE_DIR})\n    glog_report_not_found(\n      \"Could not find glog include directory, set GLOG_INCLUDE_DIR \"\n      \"to directory containing glog/logging.h\")\n  endif (NOT GLOG_INCLUDE_DIR OR\n    NOT EXISTS ${GLOG_INCLUDE_DIR})\n\n  find_library(GLOG_LIBRARY NAMES glog\n    HINTS ${GLOG_LIBRARY_DIR_HINTS}\n    PATHS ${GLOG_CHECK_LIBRARY_DIRS}\n    PATH_SUFFIXES ${GLOG_CHECK_LIBRARY_SUFFIXES})\n  if (NOT GLOG_LIBRARY OR\n      NOT EXISTS ${GLOG_LIBRARY})\n    glog_report_not_found(\n      \"Could not find glog library, set GLOG_LIBRARY \"\n      \"to full path to libglog.\")\n  endif (NOT GLOG_LIBRARY OR\n    NOT EXISTS ${GLOG_LIBRARY})\n\n  # Mark internally as found, then verify. GLOG_REPORT_NOT_FOUND() unsets\n  # if called.\n  set(GLOG_FOUND TRUE)\n\n  # Glog does not seem to provide any record of the version in its\n  # source tree, thus cannot extract version.\n\n  # Catch case when caller has set GLOG_INCLUDE_DIR in the cache / GUI and\n  # thus FIND_[PATH/LIBRARY] are not called, but specified locations are\n  # invalid, otherwise we would report the library as found.\n  if (GLOG_INCLUDE_DIR AND\n      NOT EXISTS ${GLOG_INCLUDE_DIR}/glog/logging.h)\n    glog_report_not_found(\n      \"Caller defined GLOG_INCLUDE_DIR:\"\n      \" ${GLOG_INCLUDE_DIR} does not contain glog/logging.h header.\")\n  endif (GLOG_INCLUDE_DIR AND\n    NOT EXISTS ${GLOG_INCLUDE_DIR}/glog/logging.h)\n  # TODO: This regex for glog library is pretty primitive, we use lowercase\n  #       for comparison to handle Windows using CamelCase library names, could\n  #       this check be better?\n  string(TOLOWER \"${GLOG_LIBRARY}\" LOWERCASE_GLOG_LIBRARY)\n  if (GLOG_LIBRARY AND\n      NOT \"${LOWERCASE_GLOG_LIBRARY}\" MATCHES \".*glog[^/]*\")\n    glog_report_not_found(\n      \"Caller defined GLOG_LIBRARY: \"\n      \"${GLOG_LIBRARY} does not match glog.\")\n  endif (GLOG_LIBRARY AND\n    NOT \"${LOWERCASE_GLOG_LIBRARY}\" MATCHES \".*glog[^/]*\")\n\n  glog_reset_find_library_prefix()\n\nendif(NOT GLOG_FOUND)\n\n# Set standard CMake FindPackage variables if found.\nif (GLOG_FOUND)\n  set(GLOG_INCLUDE_DIRS ${GLOG_INCLUDE_DIR})\n  set(GLOG_LIBRARIES ${GLOG_LIBRARY})\nendif (GLOG_FOUND)\n\n# If we are using an exported CMake glog target, the include directories are\n# wrapped into the target itself, and do not have to be (and are not)\n# separately specified.  In which case, we should not add GLOG_INCLUDE_DIRS\n# to the list of required variables in order that glog be reported as found.\nif (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION)\n  set(GLOG_REQUIRED_VARIABLES GLOG_LIBRARIES)\nelse()\n  set(GLOG_REQUIRED_VARIABLES GLOG_INCLUDE_DIRS GLOG_LIBRARIES)\nendif()\n\n# Handle REQUIRED / QUIET optional arguments.\ninclude(FindPackageHandleStandardArgs)\nfind_package_handle_standard_args(Glog DEFAULT_MSG\n  ${GLOG_REQUIRED_VARIABLES})\n\n# Only mark internal variables as advanced if we found glog, otherwise\n# leave them visible in the standard GUI for the user to set manually.\nif (GLOG_FOUND)\n  mark_as_advanced(FORCE GLOG_INCLUDE_DIR\n                         GLOG_LIBRARY\n                         glog_DIR) # Autogenerated by find_package(glog)\nendif (GLOG_FOUND)"
  },
  {
    "path": "gnss_comm/docker/Dockerfile",
    "content": "FROM ros:kinetic-perception\n\nENV EIGEN_VERSION=\"3.3.3\"\nENV CATKIN_WS=/root/catkin_ws\n\n      # set up thread number for building\nRUN   apt-get update && apt-get install -y \\\n      git \\\n      cmake \\\n      python-catkin-tools \\\n      libgoogle-glog-dev && \\\n      rm -rf /var/lib/apt/lists/* && \\\n      # install eigen\n      git clone https://gitlab.com/libeigen/eigen.git && \\\n      cd eigen && \\\n      git checkout tags/${EIGEN_VERSION} && \\\n      mkdir build && cd build && \\\n      cmake .. && make install && \\\n      cd ../.. && rm -rf eigen && \\\n      # create gnss_comm directory\n      mkdir -p $CATKIN_WS/src/gnss_comm/\n\n# Copy the local replica of gnss_comm\nCOPY ./ $CATKIN_WS/src/gnss_comm/\n# use the following line if you only have this dockerfile\n# RUN git clone https://github.com/HKUST-Aerial-Robotics/gnss_comm.git\n\n# Build gnss_comm\nWORKDIR $CATKIN_WS\nENV TERM xterm\nENV PYTHONIOENCODING UTF-8\nRUN catkin config \\\n      --extend /opt/ros/$ROS_DISTRO \\\n      --cmake-args \\\n        -DCMAKE_BUILD_TYPE=Release && \\\n    catkin build && \\\n    sed -i '/exec \"$@\"/i \\\n            source \"/root/catkin_ws/devel/setup.bash\"' /ros_entrypoint.sh\n"
  },
  {
    "path": "gnss_comm/docker/Makefile",
    "content": "all: help\n\nhelp:\n\t@echo \"\"\n\t@echo \"-- Help Menu\"\n\t@echo \"\"\n\t@echo \"   1. make build              - build all images\"\n\t@echo \"   1. make clean              - remove all images\"\n\t@echo \"\"\n\nbuild:\n\t@docker build --tag gnss_comm -f ./Dockerfile ..\n\nclean:\n\t@docker rmi -f gnss_comm"
  },
  {
    "path": "gnss_comm/include/gnss_comm/gnss_constant.hpp",
    "content": "/**\n* This file is part of gnss_comm.\n*\n* Copyright (C) 2021 Aerial Robotics Group, Hong Kong University of Science and Technology\n* Author: CAO Shaozu (shaozu.cao@gmail.com)\n*\n* gnss_comm is free software: you can redistribute it and/or modify\n* it under the terms of the GNU General Public License as published by\n* the Free Software Foundation, either version 3 of the License, or\n* (at your option) any later version.\n*\n* gnss_comm is distributed in the hope that it will be useful,\n* but WITHOUT ANY WARRANTY; without even the implied warranty of\n* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n* GNU General Public License for more details.\n*\n* You should have received a copy of the GNU General Public License\n* along with gnss_comm. If not, see <http://www.gnu.org/licenses/>.\n*\n* As most of the GNSS-related constants are adapted from RTKLIB, \n* the license for those part of code is claimed as follows:\n* \n* The RTKLIB software package is distributed under the following BSD 2-clause\n* license (http://opensource.org/licenses/BSD-2-Clause) and additional two\n* exclusive clauses. Users are permitted to develop, produce or sell their own\n* non-commercial or commercial products utilizing, linking or including RTKLIB as\n* long as they comply with the license.\n* \n*         Copyright (c) 2007-2020, T. Takasu, All rights reserved.\n*/\n\n#ifndef GNSS_CONSTANT_HPP_\n#define GNSS_CONSTANT_HPP_\n\n#include <iostream>\n#include <vector>\n#include <map>\n#include <set>\n#include <string>\n#include <memory>\n#include <iomanip>\n#include <Eigen/Dense>\n\nnamespace gnss_comm\n{\n    // some parameters are adapted from RTKLIB\n    #define MAXFREQ     7                   /* max N_FREQ */\n\n    #define FREQ1       1.57542E9           /* L1/E1  frequency (Hz) */\n    #define FREQ2       1.22760E9           /* L2     frequency (Hz) */\n    #define FREQ5       1.17645E9           /* L5/E5a frequency (Hz) */\n    #define FREQ6       1.27875E9           /* E6/LEX frequency (Hz) */\n    #define FREQ7       1.20714E9           /* E5b    frequency (Hz) */\n    #define FREQ8       1.191795E9          /* E5a+b  frequency (Hz) */\n    #define FREQ9       2.492028E9          /* S      frequency (Hz) */\n    #define FREQ1_GLO   1.60200E9           /* GLONASS G1 base frequency (Hz) */\n    #define DFRQ1_GLO   0.56250E6           /* GLONASS G1 bias frequency (Hz/n) */\n    #define FREQ2_GLO   1.24600E9           /* GLONASS G2 base frequency (Hz) */\n    #define DFRQ2_GLO   0.43750E6           /* GLONASS G2 bias frequency (Hz/n) */\n    #define FREQ3_GLO   1.202025E9          /* GLONASS G3 frequency (Hz) */\n    #define FREQ1_BDS   1.561098E9          /* BeiDou B1 frequency (Hz) */\n    #define FREQ2_BDS   1.20714E9           /* BeiDou B2 frequency (Hz) */\n    #define FREQ3_BDS   1.26852E9           /* BeiDou B3 frequency (Hz) */\n\n    #define SYS_NONE    0x00                /* navigation system: none */\n    #define SYS_GPS     0x01                /* navigation system: GPS */\n    #define SYS_SBS     0x02                /* navigation system: SBAS */\n    #define SYS_GLO     0x04                /* navigation system: GLONASS */\n    #define SYS_GAL     0x08                /* navigation system: Galileo */\n    #define SYS_QZS     0x10                /* navigation system: QZSS */\n    #define SYS_BDS     0x20                /* navigation system: BeiDou */\n    #define SYS_IRN     0x40                /* navigation system: IRNSS */\n    #define SYS_LEO     0x80                /* navigation system: LEO */\n    #define SYS_ALL     0xFF                /* navigation system: all */\n\n    #define T_SYS_GPS       0               /* time system: GPS time */\n    #define T_SYS_UTC       1               /* time system: UTC */\n    #define T_SYS_GLO       2               /* time system: GLONASS time */\n    #define T_SYS_GAL       3               /* time system: Galileo time */\n    #define T_SYS_QZS       4               /* time system: QZSS time */\n    #define T_SYS_BDS       5               /* time system: BeiDou time */\n\n    #ifndef N_FREQ\n    #define N_FREQ          3                   /* number of carrier frequencies */\n    #endif\n    #define N_FREQ_GLO      2                   /* number of carrier frequencies of GLONASS */\n\n    #define MIN_PRN_GPS     1                   /* min satellite PRN number of GPS */\n    #define MAX_PRN_GPS     32                  /* max satellite PRN number of GPS */\n    #define N_SAT_GPS       (MAX_PRN_GPS-MIN_PRN_GPS+1) /* number of GPS satellites */\n    #define N_SYS_GPS       1\n\n    // #define MIN_PRN_GLO     38                   /* min satellite slot number of GLONASS */ For UM4B0\n    // #define MAX_PRN_GLO     61                  /* max satellite slot number of GLONASS */  For UM4B0\n    #define MIN_PRN_GLO     1                   /* min satellite slot number of GLONASS */\n    #define MAX_PRN_GLO     27                  /* max satellite slot number of GLONASS */\n    #define N_SAT_GLO       (MAX_PRN_GLO-MIN_PRN_GLO+1) /* number of GLONASS satellites */\n    #define N_SYS_GLO       1\n\n    #define MIN_PRN_GAL     1                   /* min satellite PRN number of Galileo */\n    #define MAX_PRN_GAL     38                  /* max satellite PRN number of Galileo */\n    #define N_SAT_GAL       (MAX_PRN_GAL-MIN_PRN_GAL+1) /* number of Galileo satellites */\n    #define N_SYS_GAL       1\n\n    #define MIN_PRN_BDS     1                   /* min satellite sat number of BeiDou */\n    #define MAX_PRN_BDS     63                  /* max satellite sat number of BeiDou */\n    #define N_SAT_BDS       (MAX_PRN_BDS-MIN_PRN_BDS+1) /* number of BeiDou satellites */\n    #define N_SYS_BDS       1\n\n    #define N_SYS           (N_SYS_GPS+N_SYS_GLO+N_SYS_GAL++N_SYS_BDS) /* number of systems */\n\n    #define MAX_SAT         (N_SAT_GPS+N_SAT_GLO+N_SAT_GAL+N_SAT_BDS)\n\n    #ifndef MAXOBS\n    #define MAXOBS      64                  /* max number of obs in an epoch */\n    #endif\n\n    // solution type\n    // #define SOL_COMPUTED            0\n    // #define SOL_INSUFFICIENT_OBS    1\n    // #define NO_CONVERGENCE          2\n    // #define COV_TRACE               4\n\n    // extend solution type\n    // #define EXT_NONE                0x00\n    // #define EXT_RTK                 0x01\n    // #define EXT_IONO_BRDC           0x02\n    // #define EXT_IONO_SBAS           0x04\n    // #define EXT_IONO_IFLC           0x06\n    // #define EXT_IONO_DIFF           0x08\n\n    // position(velocity) type\n    #define PV_NONE                     0               /* no solution */\n    #define PV_FIXED_POS                1\n    #define PV_FIXED_HEIGHT             2\n    #define PV_DOPPLER_VELOCITY         8\n    #define PV_SINGLE                   16\n    #define PV_PSR_DIFF                 17\n    #define PV_WASS                     18\n    #define PV_L1_FLOAT                 32\n    #define PV_IONO_FREE_FLOAT          33\n    #define PV_NARROW_FLOW              34\n    #define PV_L1_INT                   48\n    #define PV_WIDE_INT                 49\n    #define PV_NARROW_INT               50\n    #define PV_INS                      52\n    #define PV_INS_PSR_SP               53\n    #define PV_INS_PSR_DIFF             54\n    #define PV_INS_RTK_FLOAT            55\n    #define PV_INS_RTK_FIXED            56\n\n    #define CODE_NONE   0                   /* obs code: none or unknown */\n    #define CODE_L1C    1                   /* obs code: L1C/A,G1C/A,E1C (GPS,GLO,GAL,QZS,SBS) */\n    #define CODE_L1P    2                   /* obs code: L1P,G1P    (GPS,GLO) */\n    #define CODE_L1W    3                   /* obs code: L1 Z-track (GPS) */\n    #define CODE_L1Y    4                   /* obs code: L1Y        (GPS) */\n    #define CODE_L1M    5                   /* obs code: L1M        (GPS) */\n    #define CODE_L1N    6                   /* obs code: L1codeless (GPS) */\n    #define CODE_L1S    7                   /* obs code: L1C(D)     (GPS,QZS) */\n    #define CODE_L1L    8                   /* obs code: L1C(P)     (GPS,QZS) */\n    #define CODE_L1E    9                   /* obs code: L1-SAIF    (QZS) */\n    #define CODE_L1A    10                  /* obs code: E1A        (GAL) */\n    #define CODE_L1B    11                  /* obs code: E1B        (GAL) */\n    #define CODE_L1X    12                  /* obs code: E1B+C,L1C(D+P) (GAL,QZS) */\n    #define CODE_L1Z    13                  /* obs code: E1A+B+C,L1SAIF (GAL,QZS) */\n    #define CODE_L2C    14                  /* obs code: L2C/A,G1C/A (GPS,GLO) */\n    #define CODE_L2D    15                  /* obs code: L2 L1C/A-(P2-P1) (GPS) */\n    #define CODE_L2S    16                  /* obs code: L2C(M)     (GPS,QZS) */\n    #define CODE_L2L    17                  /* obs code: L2C(L)     (GPS,QZS) */\n    #define CODE_L2X    18                  /* obs code: L2C(M+L),B1I+Q (GPS,QZS,CMP) */\n    #define CODE_L2P    19                  /* obs code: L2P,G2P    (GPS,GLO) */\n    #define CODE_L2W    20                  /* obs code: L2 Z-track (GPS) */\n    #define CODE_L2Y    21                  /* obs code: L2Y        (GPS) */\n    #define CODE_L2M    22                  /* obs code: L2M        (GPS) */\n    #define CODE_L2N    23                  /* obs code: L2codeless (GPS) */\n    #define CODE_L5I    24                  /* obs code: L5/E5aI    (GPS,GAL,QZS,SBS) */\n    #define CODE_L5Q    25                  /* obs code: L5/E5aQ    (GPS,GAL,QZS,SBS) */\n    #define CODE_L5X    26                  /* obs code: L5/E5aI+Q  (GPS,GAL,QZS,SBS) */\n    #define CODE_L7I    27                  /* obs code: E5bI,B2I   (GAL,CMP) */\n    #define CODE_L7Q    28                  /* obs code: E5bQ,B2Q   (GAL,CMP) */\n    #define CODE_L7X    29                  /* obs code: E5bI+Q,B2I+Q (GAL,CMP) */\n    #define CODE_L6A    30                  /* obs code: E6A        (GAL) */\n    #define CODE_L6B    31                  /* obs code: E6B        (GAL) */\n    #define CODE_L6C    32                  /* obs code: E6C        (GAL) */\n    #define CODE_L6X    33                  /* obs code: E6B+C,LEXS+L,B3I+Q (GAL,QZS,CMP) */\n    #define CODE_L6Z    34                  /* obs code: E6A+B+C    (GAL) */\n    #define CODE_L6S    35                  /* obs code: LEXS       (QZS) */\n    #define CODE_L6L    36                  /* obs code: LEXL       (QZS) */\n    #define CODE_L8I    37                  /* obs code: E5(a+b)I   (GAL) */\n    #define CODE_L8Q    38                  /* obs code: E5(a+b)Q   (GAL) */\n    #define CODE_L8X    39                  /* obs code: E5(a+b)I+Q (GAL) */\n    #define CODE_L2I    40                  /* obs code: B1I        (CMP) */\n    #define CODE_L2Q    41                  /* obs code: B1Q        (CMP) */\n    #define CODE_L6I    42                  /* obs code: B3I        (CMP) */\n    #define CODE_L6Q    43                  /* obs code: B3Q        (CMP) */\n    #define CODE_L3I    44                  /* obs code: G3I        (GLO) */\n    #define CODE_L3Q    45                  /* obs code: G3Q        (GLO) */\n    #define CODE_L3X    46                  /* obs code: G3I+Q      (GLO) */\n    #define CODE_L1I    47                  /* obs code: B1I        (BDS) */\n    #define CODE_L1Q    48                  /* obs code: B1Q        (BDS) */\n    #define MAXCODE     48                  /* max number of obs code */\n\n    #define EARTH_ECCE_2            6.69437999014e-3    // WGS 84 (Earth eccentricity)^2 (m^2)\n    #define EARTH_MEAN_RADIUS       6371009             // Mean R of ellipsoid(m) IU Gedosey& Geophysics\n    #define EARTH_SEMI_MAJOR        6378137             // WGS 84 Earth semi-major axis (m)\n    #define EARTH_SEMI_MAJOR_GLO    6378136.0           // radius of earth (m)\n    #define EARTH_OMG_GLO           7.2921150000e-5     // GLO value of earth's rotation rate (rad/s)\n    #define EARTH_OMG_GPS           7.2921151467e-5     // GPS/GAL value of earth's rotation rate (rad/s)\n    #define EARTH_OMG_BDS           7.2921150000e-5     // BDS value of earth's rotation rate (rad/s)\n    #define MU_GPS                  3.9860050000e14     // gravitational constant (GPS)      \n    #define MU                      3.9860044180e14     // gravitational constant (GAL, BDS, GLO)    \n    #define TSTEP                   60.0                // integration step glonass ephemeris (s)\n    #define J2_GLO                  1.0826257E-3        // 2nd zonal harmonic of geopot\n    #define LIGHT_SPEED             2.99792458e8        // WGS-84 Speed of light in a vacuum (m/s)\n    #define GPS_EPHCH_JD            2444244.5           // GPS Epoch in Julian Days\n    #define EPH_VALID_SECONDS       7200                // 2 hours ephemeris validity\n    #define WEEK_SECONDS            604800              // Seconds within one week\n    #define EPSILON_KEPLER          1e-14               // Kepler equation terminate condition\n    #define MAX_ITER_KEPLER         30                  // Kepler equation maximum iteration number\n    #define EPSILON_PVT             1e-8                // PVT terminate condition\n    #define MAX_ITER_PVT            30                  // PVT maximum iteration number\n    #define RANGE_FREQ              1                   // Range measurement frequency\n    #define R2D                     (180.0/M_PI)        // radius to degree\n    #define D2R                     (M_PI/180.0)        // degree to radius\n    #define SC2RAD                  3.1415926535898     /* semi-circle to radian (IS-GPS) */\n    #define SIN_N5                  -0.0871557427476582 // sin(-5.0 deg)\n    #define COS_N5                   0.9961946980917456 // cos(-5.0 deg)\n\n    #define INVALID_CLK             999999.999999\n\n    #define MAXLEAPS                64\n    const double leaps[MAXLEAPS+1][7]={ /* leap seconds (y,m,d,h,m,s,utc-gpst) */\n        {2017,1,1,0,0,0,-18},\n        {2015,7,1,0,0,0,-17},\n        {2012,7,1,0,0,0,-16},\n        {2009,1,1,0,0,0,-15},\n        {2006,1,1,0,0,0,-14},\n        {1999,1,1,0,0,0,-13},\n        {1997,7,1,0,0,0,-12},\n        {1996,1,1,0,0,0,-11},\n        {1994,7,1,0,0,0,-10},\n        {1993,7,1,0,0,0, -9},\n        {1992,7,1,0,0,0, -8},\n        {1991,1,1,0,0,0, -7},\n        {1990,1,1,0,0,0, -6},\n        {1988,1,1,0,0,0, -5},\n        {1985,7,1,0,0,0, -4},\n        {1983,7,1,0,0,0, -3},\n        {1982,7,1,0,0,0, -2},\n        {1981,7,1,0,0,0, -1},\n        {0}\n    };\n\n    /* System identifier to system code */\n    const std::map<uint8_t, uint32_t> char2sys = \n    {\n        {'G', SYS_GPS},\n        {'C', SYS_BDS},\n        {'R', SYS_GLO},\n        {'E', SYS_GAL}\n    };\n\n    /* System map to index */\n    const std::map<uint32_t, uint32_t> sys2idx = \n    {\n        {SYS_GPS, 0},\n        {SYS_GLO, 1},\n        {SYS_GAL, 2},\n        {SYS_BDS, 3}\n    };\n\n    /* RINEX frequency encoding to frequency value */\n    const std::map<std::string, double> type2freq = \n    {\n        {\"G1\", FREQ1},\n        {\"G2\", FREQ2},\n        {\"G5\", FREQ5},\n        {\"R1\", FREQ1_GLO},\n        {\"R2\", FREQ2_GLO},\n        {\"R3\", 1.202025E9},\n        {\"R4\", 1.600995E9},\n        {\"R6\", 1.248060E9},\n        {\"E1\", FREQ1},\n        {\"E5\", FREQ5},\n        {\"E6\", FREQ6},\n        {\"E7\", FREQ7},\n        {\"E8\", FREQ8},\n        {\"C1\", FREQ1},\n        {\"C2\", FREQ1_BDS},\n        {\"C5\", FREQ5},\n        {\"C6\", FREQ3_BDS},\n        {\"C7\", FREQ2_BDS},\n        {\"C8\", FREQ8}\n    };\n\n    struct gtime_t\n    {\n        time_t time;            /* time (s) expressed by standard time_t */\n        double sec;             /* fraction of second under 1 s */\n    };\n\n    struct EphemBase\n    {\n        virtual ~EphemBase() = default;\n        uint32_t sat;                   /* satellite number */\n        gtime_t  ttr;                   /* transmission time in GPST */\n        gtime_t  toe;                   /* ephemeris reference time in GPST */\n        uint32_t health;                /* satellite health */\n        double   ura;                   /* satellite signal accuracy */\n        uint32_t iode;\n    };\n    typedef std::shared_ptr<EphemBase> EphemBasePtr;\n\n    struct GloEphem : EphemBase\n    {\n        int         freqo;              /* satellite frequency number */\n        uint32_t    age;                /* satellite age */\n        double      pos[3];             /* satellite position (ecef) (m) */\n        double      vel[3];             /* satellite velocity (ecef) (m/s) */\n        double      acc[3];             /* satellite acceleration (ecef) (m/s^2) */\n        double      tau_n, gamma;       /* SV clock bias (s)/relative freq bias */\n        double      delta_tau_n;        /* delay between L1 and L2 (s) */\n    };\n    typedef std::shared_ptr<GloEphem> GloEphemPtr;\n\n    struct Ephem : EphemBase\n    {\n        gtime_t     toc;                    /* clock correction reference time in GPST */\n        double      toe_tow;                /* toe seconds within the week */\n        uint32_t    week;\n        uint32_t    iodc;\n        uint32_t    code;\n        double      A, e, i0, omg, OMG0, M0, delta_n, OMG_dot, i_dot;       /* SV orbit parameters */\n        double      cuc, cus, crc, crs, cic, cis;\n        double      af0, af1, af2;          /* SV clock parameters */\n        double      tgd[2];                 /* group delay parameters */\n                                            /* GPS    :tgd[0]=TGD */\n                                            /* GAL    :tgd[0]=BGD E5a/E1,tgd[1]=BGD E5b/E1 */\n                                            /* BDS    :tgd[0]=BGD1,tgd[1]=BGD2 */\n        double A_dot, n_dot;                /* Adot,ndot for CNAV */\n    };\n    typedef std::shared_ptr<Ephem> EphemPtr;\n\n    struct Obs                                           /* observation data record */\n    {\n        gtime_t     time;                                /* receiver sampling time (GPST) */\n        uint32_t    sat;                                 /* satellite number */\n        std::vector<double> freqs;                       /* received satellite frequencies */\n        std::vector<double> CN0;                         /* signal strength */\n        std::vector<uint8_t> LLI;                        /* signal lost-lock indictor */\n        std::vector<uint8_t> code;                       /* code indicator (CODE_???) */\n        std::vector<double> psr;                         /* observation data pseudorange (m) */\n        std::vector<double> psr_std;                     /* pseudorange std (m) */\n        std::vector<double> cp;                          /* observation data carrier-phase (cycle) */\n        std::vector<double> cp_std;                      /* carrier-phase std (cycle) */\n        std::vector<double> dopp;                        /* observation data doppler frequency (Hz) */\n        std::vector<double> dopp_std;                    /* doppler std (Hz) */\n        std::vector<uint8_t> 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) */\n    };\n    typedef std::shared_ptr<Obs> ObsPtr;\n\n    struct TimePulseInfo                /* time pulse(PPS) information */\n    {\n        gtime_t time;\n        bool utc_based;\n        uint32_t time_sys;\n    };\n    typedef std::shared_ptr<TimePulseInfo> TimePulseInfoPtr;\n\n    struct BestSat\n    {\n        gtime_t     time;\n        uint32_t    sat;\n        uint8_t     status;\n        std::set<double> freq_used;      /* frequency used in solution */\n    };\n    typedef std::shared_ptr<BestSat> BestSatPtr;\n\n    struct BestPos\n    {\n        gtime_t     time;\n        uint32_t    sol_status;\n        uint32_t    pos_type;\n        double      lat, lon, hgt;\n        double      lat_sigma, lon_sigma, hgt_sigma;\n        double      undulation;\n        double      diff_age;\n        double      sol_age;\n        uint8_t     num_svs;\n        uint8_t     num_soln_svs;\n        uint8_t     ext_sol_stat;\n        uint8_t     sig_mask;\n    };\n    typedef std::shared_ptr<BestPos> BestPosPtr;\n\n    struct BestVel\n    {\n        gtime_t     time;\n        uint32_t    sol_status;\n        uint32_t    vel_type;\n        double      latency;\n        double      age;\n        double      hor_speed, vel_speed;\n        double      direction2north_degree;\n    };\n    typedef std::shared_ptr<BestVel> BestVelPtr;\n\n    struct BestXYZ\n    {\n        gtime_t     time;\n        uint32_t    pos_sol_status;\n        uint32_t    pos_type;\n        double      pos[3], pos_sigma[3];\n        uint32_t    vel_sol_status;\n        uint32_t    vel_type;\n        double      vel[3], vel_sigma[3];\n        double      vel_latency;\n        double      diff_age;\n        double      sol_age;\n        uint8_t     num_svs;\n        uint8_t     num_soln_svs;\n        uint8_t     ext_sol_stat;\n        uint8_t     sig_mask;\n    };\n    typedef std::shared_ptr<BestXYZ> BestXYZPtr;\n\n    struct PVTSolution\n    {\n        gtime_t time;\n        uint8_t fix_type;   // 0-no fix, 1-dead reckoning only, 2-2D, 3-3D, 4-GNSS+(1), 5-time only fix \n        bool valid_fix;\n        bool diff_soln;     // whether differential applied\n        uint8_t carr_soln;  // 0-no cp solution, 1-float, 2-fixed\n        uint8_t num_sv;\n        double lat, lon, hgt;\n        double hgt_msl;     // height above mean sea level\n        double h_acc, v_acc; // horizontal and vertical accuracy\n        double p_dop;       // position DOP\n        double vel_n, vel_e, vel_d; // velocity in NED frame\n        double vel_acc;         // velocity accuracy\n    };\n    typedef std::shared_ptr<PVTSolution> PVTSolutionPtr;\n\n    struct PsrPos\n    {\n        gtime_t     time;\n        uint32_t    sol_status;\n        uint32_t    pos_type;\n        double      lat, lon, hgt;\n        double      lat_sigma, lon_sigma, hgt_sigma;\n        double      undulation;\n        double      diff_age;\n        double      sol_age;\n        uint8_t     num_svs;\n        uint8_t     num_soln_svs;\n        uint8_t     ext_sol_stat;\n        uint8_t     sig_mask;\n    };\n    typedef std::shared_ptr<PsrPos> PsrPosPtr;\n\n    struct PsrVel\n    {\n        gtime_t     time;\n        uint32_t    sol_status;\n        uint32_t    vel_type;\n        double      latency;\n        double      age;                /* differential age */\n        double      hor_spd;\n        double      trk_gnd;\n        double      vert_spd;\n    };\n    typedef std::shared_ptr<PsrVel> PsrVelPtr;\n\n    struct SvInfo\n    {\n        gtime_t     time;\n        uint32_t    sat;\n        uint32_t    freqo;\n        bool        health;\n        double      elev_degree;\n        double      az_degree;\n        uint32_t    sig_mask;\n    };\n    typedef std::shared_ptr<SvInfo> SvInfoPtr;\n\n    struct TECEpoch\n    {\n        gtime_t time;       // epoch time\n        double rb;          // earth radius (km)\n        double lla_start[3];\n        double lla_end[3];\n        double lla_interval[3];\n        std::vector<double> data;  // num_hgt * num_lat * num_lon\n        std::vector<double> rms;   // num_hgt * num_lat * num_lon\n    };\n    typedef std::shared_ptr<TECEpoch> TECEpochPtr;\n\n    struct EphemerideEpoch\n    {\n        gtime_t time;\n        std::map<uint32_t, Eigen::Vector3d> sat2pos;\n        std::map<uint32_t, double> sat2clk;\n    };\n    typedef std::shared_ptr<EphemerideEpoch> EphemerideEpochPtr;\n\n    struct SatState\n    {\n        uint32_t sat_id;\n        gtime_t  ttx;\n        Eigen::Vector3d pos;\n        Eigen::Vector3d vel;\n        double dt;\n        double ddt;\n        double tgd;\n    };\n    typedef std::shared_ptr<SatState> SatStatePtr;\n}   // namespace gnss_comm\n\n#endif\n\n\n\n\n"
  },
  {
    "path": "gnss_comm/include/gnss_comm/gnss_ros.hpp",
    "content": "/**\n* This file is part of gnss_comm.\n*\n* Copyright (C) 2021 Aerial Robotics Group, Hong Kong University of Science and Technology\n* Author: CAO Shaozu (shaozu.cao@gmail.com)\n*\n* gnss_comm is free software: you can redistribute it and/or modify\n* it under the terms of the GNU General Public License as published by\n* the Free Software Foundation, either version 3 of the License, or\n* (at your option) any later version.\n*\n* gnss_comm is distributed in the hope that it will be useful,\n* but WITHOUT ANY WARRANTY; without even the implied warranty of\n* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n* GNU General Public License for more details.\n*\n* You should have received a copy of the GNU General Public License\n* along with gnss_comm. If not, see <http://www.gnu.org/licenses/>.\n*/\n\n#ifndef GNSS_ROS_HPP_\n#define GNSS_ROS_HPP_\n\n#include <ros/ros.h>\n#include <gnss_comm/GnssTimeMsg.h>\n#include <gnss_comm/GnssEphemMsg.h>\n#include <gnss_comm/GnssGloEphemMsg.h>\n#include <gnss_comm/GnssObsMsg.h>\n#include <gnss_comm/GnssMeasMsg.h>\n#include <gnss_comm/GnssBestXYZMsg.h>\n#include <gnss_comm/GnssPVTSolnMsg.h>\n#include <gnss_comm/GnssSvsMsg.h>\n#include <gnss_comm/GnssTimePulseInfoMsg.h>\n#include <gnss_comm/StampedFloat64Array.h>\n\n#include \"gnss_constant.hpp\"\n#include \"gnss_utility.hpp\"\n\nnamespace gnss_comm\n{\n    /* convert Ephem struct to ros message ---------------------------------------\n    * args   : Ephem &  ephem      I   Ephemeris\n    * return : cooresponding ephemeris message\n    *-----------------------------------------------------------------------------*/\n    GnssEphemMsg ephem2msg(const EphemPtr &ephem_ptr);\n\n    /* parse Ephem struct from ros message ----------------------------------------------\n    * args   : GnssEphemConstPtr &  gnss_ephem_msg  I   Ephemeris ros message\n    * return : cooresponding ephemeris struct\n    *-----------------------------------------------------------------------------------*/\n    EphemPtr msg2ephem(const GnssEphemMsgConstPtr &gnss_ephem_msg);\n\n    /* convert GloEphem struct to ros message ---------------------------------------\n    * args   : GloEphem &  glo_ephem      I   GLONASS Ephemeris\n    * return : cooresponding ephemeris message\n    *-----------------------------------------------------------------------------*/\n    GnssGloEphemMsg glo_ephem2msg(const GloEphemPtr &glo_ephem_ptr);\n\n    /* parse GloEphem struct from ros message ----------------------------------------------\n    * args   : GnssGloEphemConstPtr & gnss_glo_ephem_msg  I   GLONASS Ephemeris ros message\n    * return : cooresponding GLONASS ephemeris struct\n    *--------------------------------------------------------------------------------------*/\n    GloEphemPtr msg2glo_ephem(const GnssGloEphemMsgConstPtr &gnss_glo_ephem_msg);\n\n    /* convert GNSS measurements to ros message ----------------------------------\n    * args   : std::vector<ObsPtr> & meas      I   GNSS measurements\n    * return : cooresponding GNSS measurement message\n    *-----------------------------------------------------------------------------*/\n    GnssMeasMsg meas2msg(const std::vector<ObsPtr> &meas);\n\n    /* parse GNSS measurements from ros message ----------------------------------\n    * args   : GnssMeasConstPtr & gnss_meas_msg      I   GNSS measurement message\n    * return : cooresponding GNSS measurements\n    *-----------------------------------------------------------------------------*/\n    std::vector<ObsPtr> msg2meas(const GnssMeasMsgConstPtr &gnss_meas_msg);\n\n    /* convert GNSS time pulse information to ros message ----------------------------------\n    * args   : TimePulseInfoPtr tp_info      I   GNSS time pulse information\n    * return : cooresponding GNSS time pulse information message\n    *-----------------------------------------------------------------------------*/\n    GnssTimePulseInfoMsg tp_info2msg(const TimePulseInfoPtr &tp_info);\n\n    /* parse GNSS time pulse information from ros message ---------------------------------\n    * args   : GnssTimePulseInfoMsgConstPtr gnss_tp_info_msg   I   GNSS time pulse message\n    * return : cooresponding GNSS time pulse information\n    *-----------------------------------------------------------------------------*/\n    TimePulseInfoPtr msg2tp_info(const GnssTimePulseInfoMsgConstPtr &gnss_tp_info_msg);\n\n    /* convert GNSS best estimated xyz to ros message ----------------------------\n    * args   : BestXYZ & best_xyz      I   GNSS estimated xyz in ECEF\n    * return : cooresponding GNSS best xyz message\n    *-----------------------------------------------------------------------------*/\n    GnssBestXYZMsg best_xyz2msg(const BestXYZPtr &best_xyz);\n\n    /* convert GNSS PVT solution to ros message -----------------------------------\n    * args   : PVTSolutionPtr & pvt_soln      I   GNSS PVT solution\n    * return : cooresponding GNSS PVT solution message\n    *-----------------------------------------------------------------------------*/\n    GnssPVTSolnMsg pvt2msg(const PVTSolutionPtr &pvt_soln);\n\n    /* parse GNSS PVT solution from ros message ---------------------------------\n    * args   : GnssPVTSolnMsgConstPtr pvt_msg   I   GNSS PVT solution message\n    * return : cooresponding GNSS PVT solution\n    *-----------------------------------------------------------------------------*/\n    PVTSolutionPtr msg2pvt(const GnssPVTSolnMsgConstPtr &pvt_msg);\n\n    /* convert GNSS space vehicles' info to ros message ----------------------------\n    * args   : std::vector<SvInfo> & svs      I   GNSS space vehicles' info\n    * return : cooresponding GNSS space vehicle info message\n    *-----------------------------------------------------------------------------*/\n    GnssSvsMsg svs2msg(const std::vector<SvInfo> &svs);\n\n}   // namespace gnss_comm\n\n#endif"
  },
  {
    "path": "gnss_comm/include/gnss_comm/gnss_spp.hpp",
    "content": "/**\n* This file is part of gnss_comm.\n*\n* Copyright (C) 2021 Aerial Robotics Group, Hong Kong University of Science and Technology\n* Author: CAO Shaozu (shaozu.cao@gmail.com)\n*\n* gnss_comm is free software: you can redistribute it and/or modify\n* it under the terms of the GNU General Public License as published by\n* the Free Software Foundation, either version 3 of the License, or\n* (at your option) any later version.\n*\n* gnss_comm is distributed in the hope that it will be useful,\n* but WITHOUT ANY WARRANTY; without even the implied warranty of\n* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n* GNU General Public License for more details.\n*\n* You should have received a copy of the GNU General Public License\n* along with gnss_comm. If not, see <http://www.gnu.org/licenses/>.\n*/\n\n#ifndef GNSS_SPS_HPP_\n#define GNSS_SPS_HPP_\n\n#include <Eigen/Dense>\n#include \"gnss_constant.hpp\"\n\nnamespace gnss_comm\n{\n    /* filter observation, only keep L1-observed Obs -----------------------------\n    * args   : std::vector<ObsPtr>&         obs                 I   GNSS observation data\n    *          std::vector<EphemBasePtr>&   ephems              I   GNSS ephemeris data\n    * args   : std::vector<ObsPtr>&         L1_obs              I   GNSS L1 observation data\n    *          std::vector<EphemBasePtr>&   L1_ephems           I   GNSS L1 ephemeris data\n    * return : void\n    *-----------------------------------------------------------------------------*/\n    void filter_L1(const std::vector<ObsPtr> &obs, const std::vector<EphemBasePtr> &ephems, \n        std::vector<ObsPtr> &L1_obs, std::vector<EphemBasePtr> &L1_ephems);\n\n    /* calculate satellite states -----------------------------------------------\n    * args   : std::vector<ObsPtr>&         obs                 I   GNSS observation data\n    *          std::vector<EphemBasePtr>&   ephems              I   GNSS ephemeris data\n    * return : std::vector<SatStatePtr>     satellite states\n    *-----------------------------------------------------------------------------*/\n    std::vector<SatStatePtr> sat_states(const std::vector<ObsPtr> &obs, \n        const std::vector<EphemBasePtr> &ephems);\n    \n    /* calculate pseudo-range residual and Jacobian -------------------------------\n    * args   : Eigen::Matrix<double, 7, 1>&     rcv_state          I   receiver state\n    *          std::vector<ObsPtr>&             obs                I   GNSS observations\n    *          std::vector<SatStatePtr>&        all_sv_states      I   satellite states\n    *          std::vector<double>&             iono_params        I   ionospheric parameters\n    *          Eigen::VectorXd&                 res                O   pseudo-range residual\n    *          Eigen::MatrixXd&                 J                  O   Jacobian\n    *          std::vector<Eigen::Vector2d>&    atmos_delay        O   ion and tro delay\n    *          std::vector<Eigen::Vector2d>&    all_sv_azel        O   satellite azimuth and elevation (radius)\n    * return : void\n    *-----------------------------------------------------------------------------*/\n    void psr_res(const Eigen::Matrix<double, 7, 1> &rcv_state, const std::vector<ObsPtr> &obs, \n        const std::vector<SatStatePtr> &all_sv_states, const std::vector<double> &iono_params, \n        Eigen::VectorXd &res, Eigen::MatrixXd &J, std::vector<Eigen::Vector2d> &atmos_delay, \n        std::vector<Eigen::Vector2d> &all_sv_azel);\n\n    /* positioning by pseudo-range localization ----------------------------------\n    * args   : std::vector<ObsPtr>&         obs         I   GNSS observation data\n    *          std::vector<EphemBasePtr>&   ephems      I   GNSS ephemeris data\n    *          std::vector<double>&         iono_params I   ionosphere parameters\n    * return : receiver position in ECEF and four clock bias for 4 constellations \n    *-----------------------------------------------------------------------------*/\n    Eigen::Matrix<double, 7, 1> psr_pos(const std::vector<ObsPtr> &obs, \n        const std::vector<EphemBasePtr> &ephems, const std::vector<double> &iono_params);\n\n    /* calculate doppler residual and Jacobian -------------------------------\n    * args   : Eigen::Matrix<double, 4, 1>&     rcv_state          I   receiver state\n    *          Eigen::Vector3d&                 rcv_ecef           I   receiver ECEF position\n    *          std::vector<ObsPtr>&             obs                I   GNSS observations\n    *          std::vector<SatStatePtr>&        all_sv_states      I   satellite states\n    *          Eigen::VectorXd&                 res                O   pseudo-range residual\n    *          Eigen::MatrixXd&                 J                  O   Jacobian\n    * return : void\n    *-----------------------------------------------------------------------------*/\n    void dopp_res(const Eigen::Matrix<double, 4, 1> &rcv_state, const Eigen::Vector3d &rcv_ecef,\n                  const std::vector<ObsPtr> &obs, const std::vector<SatStatePtr> &all_sv_states, \n                  Eigen::VectorXd &res, Eigen::MatrixXd &J);\n\n    /* calculate velocity by using Doppler measurement -------------------------------------------------\n    * args   : std::vector<ObsPtr>&         obs         I   GNSS observation data\n    *          std::vector<EphemBasePtr>&   ephems      I   GNSS ephemeris data\n    *          Eigen::Vector3d&             ref_ecef    IO  reference ECEF position, (0,0,0) if unknown\n    * return : receiver velocity in ECEF and clock bias changing rate \n    *----------------------------------------------------------------------------------------------------*/\n    Eigen::Matrix<double, 4, 1> dopp_vel(const std::vector<ObsPtr> &obs, \n        const std::vector<EphemBasePtr> &ephems, Eigen::Vector3d &ref_ecef);\n}\n\n#endif\n"
  },
  {
    "path": "gnss_comm/include/gnss_comm/gnss_utility.hpp",
    "content": "/**\n* This file is part of gnss_comm.\n*\n* Copyright (C) 2021 Aerial Robotics Group, Hong Kong University of Science and Technology\n* Author: CAO Shaozu (shaozu.cao@gmail.com)\n*\n* gnss_comm is free software: you can redistribute it and/or modify\n* it under the terms of the GNU General Public License as published by\n* the Free Software Foundation, either version 3 of the License, or\n* (at your option) any later version.\n*\n* gnss_comm is distributed in the hope that it will be useful,\n* but WITHOUT ANY WARRANTY; without even the implied warranty of\n* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n* GNU General Public License for more details.\n*\n* You should have received a copy of the GNU General Public License\n* along with gnss_comm. If not, see <http://www.gnu.org/licenses/>.\n*\n* As many of the utility functions are adapted from RTKLIB, \n* the license for those part of code is claimed as follows:\n* \n* The RTKLIB software package is distributed under the following BSD 2-clause\n* license (http://opensource.org/licenses/BSD-2-Clause) and additional two\n* exclusive clauses. Users are permitted to develop, produce or sell their own\n* non-commercial or commercial products utilizing, linking or including RTKLIB as\n* long as they comply with the license.\n* \n*         Copyright (c) 2007-2020, T. Takasu, All rights reserved.\n*/\n\n#ifndef GNSS_UTILITY_HPP_\n#define GNSS_UTILITY_HPP_\n\n#include <iostream>\n#include <fstream>\n#include <iterator>\n#include <vector>\n#include <string>\n#include <iomanip>\n#include <unistd.h>\n#include <cmath>\n#include <time.h>\n#include <Eigen/Dense>\n#include <glog/logging.h>\n\n#include \"gnss_constant.hpp\"\n\nnamespace gnss_comm\n{\n    // some of the following functions are adapted from RTKLIB\n\n    /* satellite system+prn/slot number to satellite number ------------------------\n    * convert satellite system+prn/slot number to satellite number\n    * args   : uint32_t    sys       I   satellite system (SYS_GPS,SYS_GLO,...)\n    *          uint32_t    prn       I   satellite prn/slot number\n    * return : satellite number (0:error)\n    *-----------------------------------------------------------------------------*/\n    uint32_t sat_no(uint32_t sys, uint32_t prn);\n\n    /* satellite number to satellite system ----------------------------------------\n    * convert satellite number to satellite system\n    * args   : uint32_t    sat       I   satellite number (1-MAXSAT)\n    *          uint32_t    *prn      IO  satellite prn/slot number (NULL: no output)\n    * return : satellite system (SYS_GPS,SYS_GLO,...)\n    *-----------------------------------------------------------------------------*/\n    uint32_t satsys(uint32_t sat, uint32_t *prn);\n\n    /* convert calendar day/time to time -------------------------------------------\n    * convert calendar day/time to gtime_t struct\n    * args   : double *ep       I   day/time {year,month,day,hour,min,sec}\n    * return : gtime_t struct\n    * notes  : proper in 1970-2037 or 1970-2099 (64bit time_t)\n    *-----------------------------------------------------------------------------*/\n    gtime_t epoch2time(const double *ep);\n    /* time to calendar day/time ---------------------------------------------------\n    * convert gtime_t struct to calendar day/time\n    * args   : gtime_t t        I   gtime_t struct\n    *          double *ep       O   day/time {year,month,day,hour,min,sec}\n    * return : none\n    * notes  : proper in 1970-2037 or 1970-2099 (64bit time_t)\n    *-----------------------------------------------------------------------------*/\n    void time2epoch(gtime_t t, double *ep);\n\n    /* gps time to time ------------------------------------------------------------\n    * convert week and tow in gps time to gtime_t struct\n    * args   : uint32_t    week      I   week number in gps time\n    *          double      tow       I   time of week in gps time (s)\n    * return : gtime_t struct\n    *-----------------------------------------------------------------------------*/\n    gtime_t gpst2time(uint32_t week, double tow);\n    /* time to gps time ------------------------------------------------------------\n    * convert gtime_t struct to week and tow in gps time\n    * args   : gtime_t      t        I   gtime_t struct\n    *          uint32_t    *week     IO  week number in gps time (NULL: no output)\n    * return : time of week in gps time (s)\n    *-----------------------------------------------------------------------------*/\n    double time2gpst(gtime_t t, uint32_t *week);\n    /* galileo system time to time -------------------------------------------------\n    * convert week and tow in galileo system time (gst) to gtime_t struct\n    * args   : int    week      I   week number in gst\n    *          double tow       I   time of week in gst (s)\n    * return : gtime_t struct\n    *-----------------------------------------------------------------------------*/\n    gtime_t gst2time(int week, double tow);\n    /* time to galileo system time -------------------------------------------------\n    * convert gtime_t struct to week and tow in galileo system time (gst)\n    * args   : gtime_t t        I   gtime_t struct\n    *          int    *week     IO  week number in gst (NULL: no output)\n    * return : time of week in gst (s)\n    *-----------------------------------------------------------------------------*/\n    double time2gst(gtime_t t, int *week);\n    /* beidou time (bdt) to time ---------------------------------------------------\n    * convert week and tow in beidou time (bdt) to gtime_t struct\n    * args   : int    week      I   week number in bdt\n    *          double tow       I   time of week in bdt (s)\n    * return : gtime_t struct\n    *-----------------------------------------------------------------------------*/\n    gtime_t bdt2time(int week, double tow);\n    /* time to beidouo time (bdt) --------------------------------------------------\n    * convert gtime_t struct to week and tow in beidou time (bdt)\n    * args   : gtime_t t        I   gtime_t struct\n    *          int    *week     IO  week number in bdt (NULL: no output)\n    * return : time of week in bdt (s)\n    *-----------------------------------------------------------------------------*/\n    double time2bdt(gtime_t t, int *week);\n\n    /* gpstime to utc --------------------------------------------------------------\n    * convert gpstime to utc considering leap seconds\n    * args   : gtime_t t        I   time expressed in gpstime\n    * return : time expressed in utc\n    * notes  : ignore slight time offset under 100 ns\n    *-----------------------------------------------------------------------------*/\n    gtime_t gpst2utc(gtime_t t);\n\n    /* utc to gpstime --------------------------------------------------------------\n    * convert utc to gpstime considering leap seconds\n    * args   : gtime_t t        I   time expressed in utc\n    * return : time expressed in gpstime\n    * notes  : ignore slight time offset under 100 ns\n    *-----------------------------------------------------------------------------*/\n    gtime_t utc2gpst(gtime_t t);\n\n    /* datetime to Julian day -----------------------------------------------------\n    * convert datetime to Julian day\n    * args   : std::vector<double> datetime        I   datetime (year, month, day, hour, minute, second)\n    * return : Julian day corresponding to the datetime\n    * notes  : vaild range: 1900 ~ 2100\n    *-----------------------------------------------------------------------------*/\n    double julian_day(std::vector<double> datetime);\n\n    /* find the number of leap seconds at the given datetime since GPS start epoch \n    * args   : std::vector<double> datetime        I   datetime (year, month, day, hour, minute, second)\n    * return : leap seconds at the given datetime\n    *-----------------------------------------------------------------------------*/\n    uint32_t leap_seconds_from_GPS_epoch(std::vector<double> datetime);\n\n    /* time to day of year ---------------------------------------------------------\n    * convert time to day of year\n    * args   : gtime_t t        I   gtime_t struct\n    * return : day of year (days)\n    *-----------------------------------------------------------------------------*/\n    double time2doy(gtime_t time);\n    \n    /* time difference between two timestamps -------------------------------------\n    * args   : gtime_t t1        I   time 1\n    *          gtime_t t2        I   time 2\n    * return : time difference between t1 and t2 in seconds, e.g. (t1 - t2)\n    *-----------------------------------------------------------------------------*/\n    double time_diff(gtime_t t1, gtime_t t2);\n\n    /* add specific seconds to a timestamp -------------------------------------\n    * args   : gtime_t t        I   base timestamp\n    *          double  sec      I   duration to add\n    * return : timestamp which equals to `sec` seconds past t\n    *-----------------------------------------------------------------------------*/\n    gtime_t time_add(gtime_t t, double sec);\n\n    /* time to timestamp in seconds -----------------------------------------------\n    * convert gtime_t struct to timestamp in seconds\n    * args   : gtime_t t        I   gtime_t struct\n    * return : timestamp (s)\n    *-----------------------------------------------------------------------------*/\n    double time2sec(gtime_t time);\n\n    /* timestamp to time in seconds -----------------------------------------------\n    * convert timestamp to gtime_t struct in seconds\n    * args   : timestamp sec        I   timestamp in seconds\n    * return : gtime_t struct\n    *-----------------------------------------------------------------------------*/\n    gtime_t sec2time(const double sec);\n\n    /* Geodetic coordinate to ECEF coordinate -------------------------------------\n    * convert a point in geodetic coordinate to ECEF coordinate\n    * args   : lla        I   Eigen::Vector3d Latitude, Longitude, Altitude (degree)\n    * return : Corresponding ECEF coordinate\n    *-----------------------------------------------------------------------------*/\n    Eigen::Vector3d geo2ecef(const Eigen::Vector3d &lla);\n\n    /* ECEF coordinate to Geodetic coordinate ----------------------------------------------\n    * convert a point in ECEF coordinate to geodetic coordinate\n    * args   : xyz        I   Eigen::Vector3d\n    * return : Corresponding geodetic coordinate, Longitude, Latitude, Altitude in degree\n    *-------------------------------------------------------------------------------------*/\n    Eigen::Vector3d ecef2geo(const Eigen::Vector3d &xyz);\n\n    /* solve Kepler equation  ------------------------------------------------------*/\n    double Kepler(const double mk, const double es);\n\n    /* calculate satellite clock drift  -------------------------------------------\n    * calculate satellite position in ECEF (GPS, GAL, BDS)\n    * args   : gtime_t     curr_time       I   current time\n    *          Ephem       ephem           I   satellite ephemeris (GPS, BDS, GAL)\n    * return : satellite clock drift in seconds\n    *-----------------------------------------------------------------------------*/\n    double eph2svdt(const gtime_t &curr_time, const EphemPtr ephem_ptr);\n\n    /* calculate satellite position in ECEF  ----------------------------------------\n    * calculate satellite position in ECEF (GPS, GAL, BDS)\n    * args   : gtime_t     curr_time       I   current time\n    *          Ephem       ephem           I   satellite ephemeris (GPS, BDS, GAL)\n    *          double*     svdt            IO  satellite clock drift (NULL: no output)\n    * return : satellite position in ECEF frame\n    *-----------------------------------------------------------------------------*/\n    Eigen::Vector3d eph2pos(const gtime_t &curr_time, const EphemPtr ephem_ptr, double *svdt);\n\n    /* calculate satellite velocity in ECEF  ----------------------------------------\n    * calculate satellite velocity in ECEF (GPS, GAL, BDS)\n    * args   : gtime_t     curr_time       I   current time\n    *          Ephem       ephem           I   satellite ephemeris (GPS, BDS, GAL)\n    *          double*     svddt           IO  satellite clock drift rate (NULL: no output)\n    * return : satellite velocity in ECEF frame\n    *-----------------------------------------------------------------------------*/\n    Eigen::Vector3d eph2vel(const gtime_t &curr_time, const EphemPtr ephem, double *svddt);\n\n    /* glonass orbit differential equations --------------------------------------*/\n    void deq(const Eigen::Vector3d &pos, const Eigen::Vector3d &vel, const Eigen::Vector3d &acc,\n             Eigen::Vector3d &pos_dot, Eigen::Vector3d &vel_dot);\n    \n    /* glonass position and velocity by numerical integration (RK4) --------------------*/\n    void glo_orbit(double dt, Eigen::Vector3d &pos, Eigen::Vector3d &vel, const Eigen::Vector3d &acc);\n\n    /* glonass ephemeris to satellite clock bias -----------------------------------\n    * compute satellite clock bias with glonass ephemeris\n    * args   : gtime_t curr_time    I   time by satellite clock (gpst)\n    *          geph_t  geph         I   glonass ephemeris\n    * return : satellite clock bias (s)\n    *-----------------------------------------------------------------------------*/\n    double geph2svdt(const gtime_t &curr_time, const GloEphemPtr geph_ptr);\n\n    /* glonass ephemeris to satellite position and clock bias ----------------------\n    * compute satellite position and clock bias with glonass ephemeris\n    * args   : gtime_t curr_time    I       time (gpst)\n    *          geph_t  geph         I       glonass ephemeris\n    *          double *svdt         IO      output satellite clock bias in second (NULL: no output)\n    * return : satellite position in ECEF      Eigen::Vector3d\n    *-----------------------------------------------------------------------------*/\n    Eigen::Vector3d geph2pos(const gtime_t &curr_time, const GloEphemPtr geph_ptr, double *svdt);\n\n    /* glonass ephemeris to satellite position and clock bias ----------------------\n    * compute satellite velocity and clock bias change rate with glonass ephemeris\n    * args   : gtime_t curr_time    I       time (gpst)\n    *          geph_t  geph         I       glonass ephemeris\n    *          double *svddt        IO      output satellite clock bias change rate(s/s) (NULL: no output)\n    * return : satellite velocity in ECEF      Eigen::Vector3d\n    *-----------------------------------------------------------------------------*/\n    Eigen::Vector3d geph2vel(const gtime_t &curr_time, const GloEphemPtr geph_ptr, double *svddt);\n\n    /* satellite azimuth/elevation angle -------------------------------------------\n    * compute satellite azimuth/elevation angle\n    * args   : Eigen::Vector3d rev_pos  I   receiver position in ECEF\n    *          Eigen::Vector3d sat_pos  I   satellilte position in ECEF\n    *          double*         azel     O   azimuth/elevation {az,el} (rad) (NULL: no output)\n    *                               (0.0<=azel[0]<2*pi,-pi/2<=azel[1]<=pi/2)\n    *-----------------------------------------------------------------------------*/\n    void sat_azel(const Eigen::Vector3d &rev_pos, const Eigen::Vector3d &sat_pos, double *azel);\n\n    /* transform ecef vector to local tangental coordinate -------------------------\n    * transform ecef vector to local tangental coordinate\n    * args   : Eigen::Vector3d pos_lla      I   geodetic position {lat,lon} (degree)\n    *          Eigen::Vector3d v_ecef       I   vector in ecef coordinate {x,y,z}\n    * return : vector in local tangental coordinate {e,n,u}\n    *-----------------------------------------------------------------------------*/\n    Eigen::Vector3d ecef2enu(const Eigen::Vector3d &pos_lla, const Eigen::Vector3d &v_ecef);\n\n    /* calculate rotation from ENU frame to ECEF via reference geodetic coordinate  ---\n    * args   : Eigen::Vector3d ref_geo      I   reference position in geodetic frame\n    * return : rotation matrix from ENU frame to ECEF frame\n    *-----------------------------------------------------------------------------*/\n    Eigen::Matrix3d geo2rotation(const Eigen::Vector3d &ref_geo);\n\n    /* calculate rotation from ENU frame to ECEF via reference ECEF coordinate  ---\n    * args   : Eigen::Vector3d ref_ecef      I   reference position in ECEF\n    * return : rotation matrix from ENU frame to ECEF frame\n    *-----------------------------------------------------------------------------*/\n    Eigen::Matrix3d ecef2rotation(const Eigen::Vector3d &ref_ecef);\n\n    /* calculate signal delay caused by troposphere ------------------------------\n    * calculate signal delay caused by troposphere\n    * args   : gtime_t time                 I   time (gpst)\n    * args   : Eigen::Vector3d rev_lla      I   receiver position in geodetic coordinate\n    *          double*         azel         I   satellite azimuth/elevation angle in radius\n    * return : delay expressed by light travel distance (m)\n    *-----------------------------------------------------------------------------*/\n    double calculate_trop_delay(gtime_t time, const Eigen::Vector3d &rev_lla, const double *azel);\n                            \n    /* calculate signal delay caused by ionosphere ------------------------------\n    * calculate signal delay caused by ionosphere\n    * args   : gtime_t              time            I   time (gpst)\n    *          std::vector<double>  ion_parameters  I   ionosphere parameters {a0,a1,a2,a3,b0,b1,b2,b3}\n    *          Eigen::Vector3d      rev_lla         I   receiver position in LLA (lat, lon, hgt) (deg, m)\n    *          double*              azel            I   satellite azimuth/elevation angle (rad)\n    * return : ionospheric delay (L1) (m)\n    *-----------------------------------------------------------------------------*/\n    double calculate_ion_delay(gtime_t t, const std::vector<double> &ion_parameters, \n                               const Eigen::Vector3d &rev_lla, const double *azel);\n\n    /* satellite RINEX identifier from RTKLIB satellite id ------------------------------\n    * args   : uint32_t             sat_no          I   satellite id in RTKLIB\n    * return : satellite RINEX identifier\n    *-----------------------------------------------------------------------------*/\n    std::string sat2str(uint32_t sat_no);\n\n    /* satellite RINEX identifier to RTKLIB satellite id ------------------------------\n    * args   : std::string             &sat_no      I   satellite identifier in RINEX\n    * return : satellite id in RTKLIB\n    *-----------------------------------------------------------------------------*/\n    uint32_t str2sat(const std::string &sat_str);\n\n    /* Get L1 frequency  ---------------------------------------------------------\n    * args   : ObsPtr&             obs      I   GNSS observation\n    *          int*                l1_idx   I/O L1 frequency index (NULL if not required)\n    * return : L1 frequency (negative if L1 frequency not exists)\n    *-----------------------------------------------------------------------------*/\n    double L1_freq(const ObsPtr &obs, int *l1_idx);\n\n    /* execute the given command  ---------------------------------------------------------\n    * args   : std::string    cmd        I   command to execute\n    * return : execute result \n    *-------------------------------------------------------------------------------------*/\n    std::string exec(const std::string &cmd);\n\n}   // namespace gnss_comm\n\n#endif\n\n"
  },
  {
    "path": "gnss_comm/include/gnss_comm/rinex_helper.hpp",
    "content": "/**\n* This file is part of gnss_comm.\n*\n* Copyright (C) 2021 Aerial Robotics Group, Hong Kong University of Science and Technology\n* Author: CAO Shaozu (shaozu.cao@gmail.com)\n*\n* gnss_comm is free software: you can redistribute it and/or modify\n* it under the terms of the GNU General Public License as published by\n* the Free Software Foundation, either version 3 of the License, or\n* (at your option) any later version.\n*\n* gnss_comm is distributed in the hope that it will be useful,\n* but WITHOUT ANY WARRANTY; without even the implied warranty of\n* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n* GNU General Public License for more details.\n*\n* You should have received a copy of the GNU General Public License\n* along with gnss_comm. If not, see <http://www.gnu.org/licenses/>.\n*/\n\n#ifndef RINEX_HELPER_HPP_\n#define RINEX_HELPER_HPP_\n\n#include <iostream>\n#include <fstream>\n#include <vector>\n#include <cmath>\n#include <map>\n#include <time.h>\n#include <glog/logging.h>\n\n#include \"gnss_constant.hpp\"\n#include \"gnss_utility.hpp\"\n\nnamespace gnss_comm\n{\n    /* parse ephemeris from RINEX navigation file  ----------------------------------------------------------\n    * args   : std::string                                  rinex_filepath        I   RINEX file path\n    *          std::map<uint32_t, std::vecot<EphemBase>>&   sat2ephem             IO  satellite number to ephemeris\n    * return : None\n    *---------------------------------------------------------------------------------------------*/\n    void rinex2ephems(const std::string &rinex_filepath, \n        std::map<uint32_t, std::vector<EphemBasePtr>> &sat2ephem);\n    \n    /* parse GNSS measurement from RINEX observation file  ---------------------------------------\n    * args   : std::string                          rinex_filepath        I   RINEX file path\n    *          std::vector<std::vector<ObsPtr>>&    rinex_meas            IO  GNSS measurement in time order\n    * return : None\n    *---------------------------------------------------------------------------------------------*/\n    void rinex2obs(const std::string &rinex_filepath, \n        std::vector<std::vector<ObsPtr>> &rinex_meas);\n    \n    /* output GNSS measurement to RINEX file  -----------------------------------------------------\n    * args   : std::string                          rinex_filepath        I   RINEX file path\n    *          std::vector<std::vector<ObsPtr>>&    rinex_meas            I   GNSS measurement in time order\n    * return : None\n    * This function is still under experiment, many hacks here.\n    *---------------------------------------------------------------------------------------------*/\n    void obs2rinex(const std::string &rinex_filepath, \n        const std::vector<std::vector<ObsPtr>> &gnss_meas);\n\n    /* parse GPS ionosphere from RINEX navigation file  ------------------------------------------\n    * args   : std::string                      rinex_filepath        I   RINEX file path\n    *          std::vector<double>              iono_params           IO  8 ionosphere parameters (alpha 1~4, beta 1~4)\n    * return : None\n    *---------------------------------------------------------------------------------------------*/\n    void rinex2iono_params(const std::string &rinex_filepath, std::vector<double> &iono_params);\n\n\n}   // namespace gnss_comm\n\n\n#endif"
  },
  {
    "path": "gnss_comm/msg/GnssBestXYZMsg.msg",
    "content": "Header header\nfloat64[] pos\nfloat64[] pos_sigma\nfloat64[] vel\nfloat64[] vel_sigma\nuint32    num_svs"
  },
  {
    "path": "gnss_comm/msg/GnssEphemMsg.msg",
    "content": "# This message contains GPS, Galileo and BeiDou ephemeris data\n\nuint32 sat\nGnssTimeMsg ttr\nGnssTimeMsg toe\nGnssTimeMsg toc\nfloat64 toe_tow\nuint32 week\nuint32 iode\nuint32 iodc\nuint32 health\nuint32 code\nfloat64 ura\nfloat64 A\nfloat64 e\nfloat64 i0\nfloat64 omg\nfloat64 OMG0\nfloat64 M0\nfloat64 delta_n\nfloat64 OMG_dot\nfloat64 i_dot\nfloat64 cuc\nfloat64 cus\nfloat64 crc\nfloat64 crs\nfloat64 cic\nfloat64 cis\nfloat64 af0\nfloat64 af1\nfloat64 af2\nfloat64 tgd0\nfloat64 tgd1\nfloat64 A_dot\nfloat64 n_dot"
  },
  {
    "path": "gnss_comm/msg/GnssGloEphemMsg.msg",
    "content": "# This message contains GLONASS ephemeris data\n\nuint32 sat\nGnssTimeMsg ttr\nGnssTimeMsg toe\nint32  freqo\nuint32 iode\nuint32 health\nuint32 age\nfloat64 ura\nfloat64 pos_x\nfloat64 pos_y\nfloat64 pos_z\nfloat64 vel_x\nfloat64 vel_y\nfloat64 vel_z\nfloat64 acc_x\nfloat64 acc_y\nfloat64 acc_z\nfloat64 tau_n\nfloat64 gamma\nfloat64 delta_tau_n"
  },
  {
    "path": "gnss_comm/msg/GnssMeasMsg.msg",
    "content": "# This message contains one-epoch measurements from multiple satellites\n\nGnssObsMsg[] meas"
  },
  {
    "path": "gnss_comm/msg/GnssObsMsg.msg",
    "content": "# This message contains one-epoch measurements from one single satellite\n\nGnssTimeMsg time    # measurement time\nuint32 sat          # satellite ID (define by `sat_no` function in `gnss_utility.hpp`)\nfloat64[] freqs     # observed frequencies [Hz]\nfloat64[] CN0       # carrier-to-noise density ratio (signal strength) [dB-Hz]\nuint8[] LLI         # lost-lock indicator (1=lost)\nuint8[] code        # channel code\nfloat64[] psr       # pseudorange measurement [m]\nfloat64[] psr_std   # pseudorange standard deviation [m]\nfloat64[] cp        # carrier phase measurement [cycle]\nfloat64[] cp_std    # carrier phase standard deviation [cycle]\nfloat64[] dopp      # Doppler measurement [Hz]\nfloat64[] dopp_std  # Doppler standard deviation [Hz]\n# tracking status. bit_0:psr valid, bit_1:cp valid, \n# bit_2:half cp valid, bit_3:half cp subtracted\nuint8[]   status    "
  },
  {
    "path": "gnss_comm/msg/GnssPVTSolnMsg.msg",
    "content": "# This message contains information of UBX-NAV-PVT message. \n# reference: [1]. UBX-18010854-R08, page 132\n\nGnssTimeMsg time    # GNSS time of the navigation epoch\n# GNSS fix type (0=no fix, 1=dead reckoning only, 2=2D-fix, 3=3D-fix, \n# 4=GNSS+dead reckoning combined, 5=time only fix)\nuint8   fix_type\nbool    valid_fix   # if fix valid (1=valid fix)\nbool    diff_soln   # if differential correction were applied (1=applied)\nuint8   carr_soln   # carrier phase range solution status (0=no carrier phase, 1=float, 2=fix)\nuint8   num_sv      # number of satellites used in the solution\nfloat64 latitude    # latitude [degree]\nfloat64 longitude   # longitude [degree]\nfloat64 altitude    # height above ellipsoid [m]\nfloat64 height_msl  # height above mean sea level [m]\nfloat64 h_acc       # horizontal accuracy estimate [m]\nfloat64 v_acc       # vertical accuracy estimate [m]\nfloat64 p_dop       # Position DOP\nfloat64 vel_n       # NED north velocity [m/s]\nfloat64 vel_e       # NED east velocity [m/s]\nfloat64 vel_d       # NED down velocity [m/s]\nfloat64 vel_acc     # speed accuracy estimate [m/s]\n"
  },
  {
    "path": "gnss_comm/msg/GnssSvsMsg.msg",
    "content": "Header header\nuint32[]    sat\nuint32[]    freqo\nbool[]      health\nfloat64[]   elev_degree\nfloat64[]   az_degree\nuint32[]    sig_mask"
  },
  {
    "path": "gnss_comm/msg/GnssTimeMsg.msg",
    "content": "# This message contains GNSS time expressed in the form of \n# GNSS week number and time of week(in seconds)\n\nuint32 week\nfloat64 tow"
  },
  {
    "path": "gnss_comm/msg/GnssTimePulseInfoMsg.msg",
    "content": "# This message contains information of UBX-TIM-TP message. \n# reference: [1]. UBX-18010854-R08, page 156\n\nGnssTimeMsg time    # GNSS time of the next time pulse\nbool utc_based      # if the time is UTC (1=UTC, 0=GNSS)\nuint32 time_sys     # GNSS time reference system, if the time base is GNSS (utc_base=0)"
  },
  {
    "path": "gnss_comm/msg/StampedFloat64Array.msg",
    "content": "# A list of float values with timestamp\n\nHeader header\nfloat64[] data"
  },
  {
    "path": "gnss_comm/package.xml",
    "content": "<?xml version=\"1.0\"?>\n<package>\n  <name>gnss_comm</name>\n  <version>1.0.0</version>\n  <description>Definitions and utility functions for GNSS</description>\n\n  <maintainer email=\"shaozu.cao@gmail.com\">CAO Shaozu</maintainer>\n\n  <license>GPLv3</license>\n\n  <buildtool_depend>catkin</buildtool_depend>\n  <build_depend>roscpp</build_depend>\n  <build_depend>rospy</build_depend>\n  <build_depend>std_msgs</build_depend>\n  <build_depend>message_generation</build_depend>\n\n  <run_depend>roscpp</run_depend>\n  <run_depend>rospy</run_depend>\n  <run_depend>std_msgs</run_depend>\n  <run_depend>message_runtime</run_depend>\n  <run_depend>message_generation</run_depend>\n\n  <!-- The export tag contains other, unspecified, tags -->\n  <export>\n  </export>\n</package>\n"
  },
  {
    "path": "gnss_comm/src/gnss_ros.cpp",
    "content": "/**\n* This file is part of gnss_comm.\n*\n* Copyright (C) 2021 Aerial Robotics Group, Hong Kong University of Science and Technology\n* Author: CAO Shaozu (shaozu.cao@gmail.com)\n*\n* gnss_comm is free software: you can redistribute it and/or modify\n* it under the terms of the GNU General Public License as published by\n* the Free Software Foundation, either version 3 of the License, or\n* (at your option) any later version.\n*\n* gnss_comm is distributed in the hope that it will be useful,\n* but WITHOUT ANY WARRANTY; without even the implied warranty of\n* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n* GNU General Public License for more details.\n*\n* You should have received a copy of the GNU General Public License\n* along with gnss_comm. If not, see <http://www.gnu.org/licenses/>.\n*/\n\n#include <gnss_comm/gnss_ros.hpp>\n\nnamespace gnss_comm\n{\n    GnssEphemMsg ephem2msg(const EphemPtr &ephem_ptr)\n    {\n        GnssEphemMsg ephem_msg;\n        uint32_t week = 0;\n        double tow = 0.0;\n        ephem_msg.sat = ephem_ptr->sat;\n        tow = time2gpst(ephem_ptr->ttr, &week);\n        ephem_msg.ttr.week = week;\n        ephem_msg.ttr.tow = tow;\n        tow = time2gpst(ephem_ptr->toe, &week);\n        ephem_msg.toe.week = week;\n        ephem_msg.toe.tow = tow;\n        tow = time2gpst(ephem_ptr->toc, &week);\n        ephem_msg.toc.week = week;\n        ephem_msg.toc.tow = tow;\n        ephem_msg.toe_tow = ephem_ptr->toe_tow;\n        ephem_msg.week = ephem_ptr->week;\n        ephem_msg.iode = ephem_ptr->iode;\n        ephem_msg.iodc = ephem_ptr->iodc;\n        ephem_msg.health = ephem_ptr->health;\n        ephem_msg.code = ephem_ptr->code;\n        ephem_msg.ura = ephem_ptr->ura;\n        ephem_msg.A = ephem_ptr->A;\n        ephem_msg.e = ephem_ptr->e;\n        ephem_msg.i0 = ephem_ptr->i0;\n        ephem_msg.omg = ephem_ptr->omg;\n        ephem_msg.OMG0 = ephem_ptr->OMG0;\n        ephem_msg.M0 = ephem_ptr->M0;\n        ephem_msg.delta_n = ephem_ptr->delta_n;\n        ephem_msg.OMG_dot = ephem_ptr->OMG_dot;\n        ephem_msg.i_dot = ephem_ptr->i_dot;\n        ephem_msg.cuc = ephem_ptr->cuc;\n        ephem_msg.cus = ephem_ptr->cus;\n        ephem_msg.crc = ephem_ptr->crc;\n        ephem_msg.crs = ephem_ptr->crs;\n        ephem_msg.cic = ephem_ptr->cic;\n        ephem_msg.cis = ephem_ptr->cis;\n        ephem_msg.af0 = ephem_ptr->af0;\n        ephem_msg.af1 = ephem_ptr->af1;\n        ephem_msg.af2 = ephem_ptr->af2;\n        ephem_msg.tgd0 = ephem_ptr->tgd[0];\n        ephem_msg.tgd1 = ephem_ptr->tgd[1];\n        ephem_msg.A_dot = ephem_ptr->A_dot;\n        ephem_msg.n_dot = ephem_ptr->n_dot;\n        return ephem_msg;\n    }\n\n    EphemPtr msg2ephem(const GnssEphemMsgConstPtr &gnss_ephem_msg)\n    {\n        EphemPtr ephem(new Ephem());\n        ephem->sat = gnss_ephem_msg->sat;\n        ephem->ttr = gpst2time(gnss_ephem_msg->ttr.week, gnss_ephem_msg->ttr.tow);\n        ephem->toe = gpst2time(gnss_ephem_msg->toe.week, gnss_ephem_msg->toe.tow);\n        ephem->toc = gpst2time(gnss_ephem_msg->toc.week, gnss_ephem_msg->toc.tow);\n        ephem->toe_tow = gnss_ephem_msg->toe_tow;\n        ephem->week = gnss_ephem_msg->week;\n        ephem->iode = gnss_ephem_msg->iode;\n        ephem->iodc = gnss_ephem_msg->iodc;\n        ephem->health = gnss_ephem_msg->health;\n        ephem->code = gnss_ephem_msg->code;\n        ephem->ura = gnss_ephem_msg->ura;\n        ephem->A = gnss_ephem_msg->A;\n        ephem->e = gnss_ephem_msg->e;\n        ephem->i0 = gnss_ephem_msg->i0;\n        ephem->omg = gnss_ephem_msg->omg;\n        ephem->OMG0 = gnss_ephem_msg->OMG0;\n        ephem->M0 = gnss_ephem_msg->M0;\n        ephem->delta_n = gnss_ephem_msg->delta_n;\n        ephem->OMG_dot = gnss_ephem_msg->OMG_dot;\n        ephem->i_dot = gnss_ephem_msg->i_dot;\n        ephem->cuc = gnss_ephem_msg->cuc;\n        ephem->cus = gnss_ephem_msg->cus;\n        ephem->crc = gnss_ephem_msg->crc;\n        ephem->crs = gnss_ephem_msg->crs;\n        ephem->cic = gnss_ephem_msg->cic;\n        ephem->cis = gnss_ephem_msg->cis;\n        ephem->af0 = gnss_ephem_msg->af0;\n        ephem->af1 = gnss_ephem_msg->af1;\n        ephem->af2 = gnss_ephem_msg->af2;\n        ephem->tgd[0] = gnss_ephem_msg->tgd0;\n        ephem->tgd[1] = gnss_ephem_msg->tgd1;\n        ephem->A_dot = gnss_ephem_msg->A_dot;\n        ephem->n_dot = gnss_ephem_msg->n_dot;\n        return ephem;\n    }\n\n    GnssGloEphemMsg glo_ephem2msg(const GloEphemPtr &glo_ephem_ptr)\n    {\n        GnssGloEphemMsg glo_ephem_msg;\n        uint32_t week = 0;\n        double tow = 0.0;\n        glo_ephem_msg.sat = glo_ephem_ptr->sat;\n        tow = time2gpst(glo_ephem_ptr->ttr, &week);\n        glo_ephem_msg.ttr.week = week;\n        glo_ephem_msg.ttr.tow = tow;\n        tow = time2gpst(glo_ephem_ptr->toe, &week);\n        glo_ephem_msg.toe.week = week;\n        glo_ephem_msg.toe.tow = tow;\n        glo_ephem_msg.freqo = glo_ephem_ptr->freqo;\n        glo_ephem_msg.iode = glo_ephem_ptr->iode;\n        glo_ephem_msg.health = glo_ephem_ptr->health;\n        glo_ephem_msg.age = glo_ephem_ptr->age;\n        glo_ephem_msg.ura = glo_ephem_ptr->ura;\n        glo_ephem_msg.pos_x = glo_ephem_ptr->pos[0];\n        glo_ephem_msg.pos_y = glo_ephem_ptr->pos[1];\n        glo_ephem_msg.pos_z = glo_ephem_ptr->pos[2];\n        glo_ephem_msg.vel_x = glo_ephem_ptr->vel[0];\n        glo_ephem_msg.vel_y = glo_ephem_ptr->vel[1];\n        glo_ephem_msg.vel_z = glo_ephem_ptr->vel[2];\n        glo_ephem_msg.acc_x = glo_ephem_ptr->acc[0];\n        glo_ephem_msg.acc_y = glo_ephem_ptr->acc[1];\n        glo_ephem_msg.acc_z = glo_ephem_ptr->acc[2];\n        glo_ephem_msg.tau_n = glo_ephem_ptr->tau_n;\n        glo_ephem_msg.gamma = glo_ephem_ptr->gamma;\n        glo_ephem_msg.delta_tau_n = glo_ephem_ptr->delta_tau_n;\n        return glo_ephem_msg;\n    }\n\n    GloEphemPtr msg2glo_ephem(const GnssGloEphemMsgConstPtr &gnss_glo_ephem_msg)\n    {\n        GloEphemPtr glo_ephem(new GloEphem());\n        glo_ephem->sat = gnss_glo_ephem_msg->sat;\n        glo_ephem->ttr = gpst2time(gnss_glo_ephem_msg->ttr.week, gnss_glo_ephem_msg->ttr.tow);\n        glo_ephem->toe = gpst2time(gnss_glo_ephem_msg->toe.week, gnss_glo_ephem_msg->toe.tow);\n        glo_ephem->freqo = gnss_glo_ephem_msg->freqo;\n        glo_ephem->iode = gnss_glo_ephem_msg->iode;\n        glo_ephem->health = gnss_glo_ephem_msg->health;\n        glo_ephem->age = gnss_glo_ephem_msg->age;\n        glo_ephem->ura = gnss_glo_ephem_msg->ura;\n        glo_ephem->pos[0] = gnss_glo_ephem_msg->pos_x;\n        glo_ephem->pos[1] = gnss_glo_ephem_msg->pos_y;\n        glo_ephem->pos[2] = gnss_glo_ephem_msg->pos_z;\n        glo_ephem->vel[0] = gnss_glo_ephem_msg->vel_x;\n        glo_ephem->vel[1] = gnss_glo_ephem_msg->vel_y;\n        glo_ephem->vel[2] = gnss_glo_ephem_msg->vel_z;\n        glo_ephem->acc[0] = gnss_glo_ephem_msg->acc_x;\n        glo_ephem->acc[1] = gnss_glo_ephem_msg->acc_y;\n        glo_ephem->acc[2] = gnss_glo_ephem_msg->acc_z;\n        glo_ephem->tau_n = gnss_glo_ephem_msg->tau_n;\n        glo_ephem->gamma = gnss_glo_ephem_msg->gamma;\n        glo_ephem->delta_tau_n = gnss_glo_ephem_msg->delta_tau_n;\n        return glo_ephem;\n    }\n\n    GnssMeasMsg meas2msg(const std::vector<ObsPtr> &meas)\n    {\n        GnssMeasMsg gnss_meas_msg;\n        for (ObsPtr obs : meas)\n        {\n            GnssObsMsg obs_msg;\n            uint32_t week = 0;\n            double tow = time2gpst(obs->time, &week);\n            obs_msg.time.week = week;\n            obs_msg.time.tow = tow;\n            obs_msg.sat     = obs->sat;\n            obs_msg.freqs   = obs->freqs;\n            obs_msg.CN0     = obs->CN0;\n            obs_msg.LLI     = obs->LLI;\n            obs_msg.code    = obs->code;\n            obs_msg.psr     = obs->psr;\n            obs_msg.psr_std = obs->psr_std;\n            obs_msg.cp      = obs->cp;\n            obs_msg.cp_std  = obs->cp_std;\n            obs_msg.dopp    = obs->dopp;\n            obs_msg.dopp_std = obs->dopp_std;\n            obs_msg.status  = obs->status;\n\n            gnss_meas_msg.meas.push_back(obs_msg);\n        }\n        return gnss_meas_msg;\n    }\n\n    std::vector<ObsPtr> msg2meas(const GnssMeasMsgConstPtr &gnss_meas_msg)\n    {\n        std::vector<ObsPtr> meas;\n        for (size_t i = 0; i < gnss_meas_msg->meas.size(); ++i)\n        {\n            GnssObsMsg obs_msg = gnss_meas_msg->meas[i];\n            ObsPtr obs(new Obs());\n            obs->time       = gpst2time(obs_msg.time.week, obs_msg.time.tow);\n            obs->sat        = obs_msg.sat;\n            obs->freqs      = obs_msg.freqs;\n            obs->CN0        = obs_msg.CN0;\n            obs->LLI        = obs_msg.LLI;\n            obs->code       = obs_msg.code;\n            obs->psr        = obs_msg.psr;\n            obs->psr_std    = obs_msg.psr_std;\n            obs->cp         = obs_msg.cp;\n            obs->cp_std     = obs_msg.cp_std;\n            obs->dopp       = obs_msg.dopp;\n            obs->dopp_std   = obs_msg.dopp_std;\n            obs->status     = obs_msg.status;\n            \n            meas.push_back(obs);\n        }\n        return meas;\n    }\n\n    GnssTimePulseInfoMsg tp_info2msg(const TimePulseInfoPtr &tp_info)\n    {\n        GnssTimePulseInfoMsg tp_info_msg;\n        tp_info_msg.time.tow = time2gpst(tp_info->time, &(tp_info_msg.time.week));\n        tp_info_msg.utc_based = tp_info->utc_based;\n        tp_info_msg.time_sys = tp_info->time_sys;\n        return tp_info_msg;\n    }\n\n    TimePulseInfoPtr msg2tp_info(const GnssTimePulseInfoMsgConstPtr &gnss_tp_info_msg)\n    {\n        TimePulseInfoPtr tp_info(new TimePulseInfo());\n        tp_info->time = gpst2time(gnss_tp_info_msg->time.week, gnss_tp_info_msg->time.tow);\n        tp_info->utc_based = gnss_tp_info_msg->utc_based;\n        tp_info->time_sys = gnss_tp_info_msg->time_sys;\n        return tp_info;\n    }\n\n    GnssBestXYZMsg best_xyz2msg(const BestXYZPtr &best_xyz)\n    {\n        GnssBestXYZMsg gnss_best_xyz_msg;\n        gnss_best_xyz_msg.header.stamp = ros::Time(time2sec(best_xyz->time));\n        gnss_best_xyz_msg.header.frame_id = \"ECEF\";\n        for (size_t i = 0; i < 3; ++i)\n        {\n            gnss_best_xyz_msg.pos.push_back(best_xyz->pos[i]);\n            gnss_best_xyz_msg.pos_sigma.push_back(best_xyz->pos_sigma[i]);\n            gnss_best_xyz_msg.vel.push_back(best_xyz->vel[i]);\n            gnss_best_xyz_msg.vel_sigma.push_back(best_xyz->vel_sigma[i]);\n        }\n        gnss_best_xyz_msg.num_svs = best_xyz->num_soln_svs;\n\n        return gnss_best_xyz_msg;\n    }\n\n    GnssPVTSolnMsg pvt2msg(const PVTSolutionPtr &pvt_soln)\n    {\n        GnssPVTSolnMsg pvt_msg;\n        pvt_msg.time.tow = time2gpst(pvt_soln->time, &(pvt_msg.time.week));\n        pvt_msg.fix_type = pvt_soln->fix_type;\n        pvt_msg.valid_fix = pvt_soln->valid_fix;\n        pvt_msg.diff_soln = pvt_soln->diff_soln;\n        pvt_msg.carr_soln = pvt_soln->carr_soln;\n        pvt_msg.num_sv = pvt_soln->num_sv;\n        pvt_msg.latitude = pvt_soln->lat;\n        pvt_msg.longitude = pvt_soln->lon;\n        pvt_msg.altitude = pvt_soln->hgt;\n        pvt_msg.height_msl = pvt_soln->hgt_msl;\n        pvt_msg.h_acc = pvt_soln->h_acc;\n        pvt_msg.v_acc = pvt_soln->v_acc;\n        pvt_msg.p_dop = pvt_soln->p_dop;\n        pvt_msg.vel_n = pvt_soln->vel_n;\n        pvt_msg.vel_e = pvt_soln->vel_e;\n        pvt_msg.vel_d = pvt_soln->vel_d;\n        pvt_msg.vel_acc = pvt_soln->vel_acc;\n        return pvt_msg;\n    }\n\n    PVTSolutionPtr msg2pvt(const GnssPVTSolnMsgConstPtr &pvt_msg)\n    {\n        PVTSolutionPtr pvt_soln(new PVTSolution());\n        pvt_soln->time = gpst2time(pvt_msg->time.week, pvt_msg->time.tow);\n        pvt_soln->fix_type = pvt_msg->fix_type;\n        pvt_soln->valid_fix = pvt_msg->valid_fix;\n        pvt_soln->diff_soln = pvt_msg->diff_soln;\n        pvt_soln->carr_soln = pvt_msg->carr_soln;\n        pvt_soln->num_sv = pvt_msg->num_sv;\n        pvt_soln->lat = pvt_msg->latitude;\n        pvt_soln->lon = pvt_msg->longitude;\n        pvt_soln->hgt = pvt_msg->altitude;\n        pvt_soln->hgt_msl = pvt_msg->height_msl;\n        pvt_soln->h_acc = pvt_msg->h_acc;\n        pvt_soln->v_acc = pvt_msg->v_acc;\n        pvt_soln->p_dop = pvt_msg->p_dop;\n        pvt_soln->vel_n = pvt_msg->vel_n;\n        pvt_soln->vel_e = pvt_msg->vel_e;\n        pvt_soln->vel_d = pvt_msg->vel_d;\n        pvt_soln->vel_acc = pvt_msg->vel_acc;\n        return pvt_soln;\n    }\n\n    GnssSvsMsg svs2msg(const std::vector<SvInfo> &svs)\n    {\n        GnssSvsMsg svs_msg;\n        if (svs.empty())    return svs_msg;\n        svs_msg.header.stamp = ros::Time(time2sec(svs[0].time));\n        for (auto & sv : svs)\n        {\n            svs_msg.sat.push_back(sv.sat);\n            svs_msg.freqo.push_back(sv.freqo);\n            svs_msg.health.push_back(sv.health);\n            svs_msg.elev_degree.push_back(sv.elev_degree);\n            svs_msg.az_degree.push_back(sv.az_degree);\n            svs_msg.sig_mask.push_back(sv.sig_mask);\n        }\n        return svs_msg;\n    }\n}   // namespace gnss_comm\n"
  },
  {
    "path": "gnss_comm/src/gnss_spp.cpp",
    "content": "/**\n* This file is part of gnss_comm.\n*\n* Copyright (C) 2021 Aerial Robotics Group, Hong Kong University of Science and Technology\n* Author: CAO Shaozu (shaozu.cao@gmail.com)\n*\n* gnss_comm is free software: you can redistribute it and/or modify\n* it under the terms of the GNU General Public License as published by\n* the Free Software Foundation, either version 3 of the License, or\n* (at your option) any later version.\n*\n* gnss_comm is distributed in the hope that it will be useful,\n* but WITHOUT ANY WARRANTY; without even the implied warranty of\n* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n* GNU General Public License for more details.\n*\n* You should have received a copy of the GNU General Public License\n* along with gnss_comm. If not, see <http://www.gnu.org/licenses/>.\n*/\n\n#include <gnss_comm/gnss_spp.hpp>\n#include <gnss_comm/gnss_utility.hpp>\n#include <glog/logging.h>\n\n#define CUT_OFF_DEGREE 15.0\n\nnamespace gnss_comm\n{\n    void filter_L1(const std::vector<ObsPtr> &obs, const std::vector<EphemBasePtr> &ephems, \n        std::vector<ObsPtr> &L1_obs, std::vector<EphemBasePtr> &L1_ephems)\n    {\n        L1_obs.clear();\n        L1_ephems.clear();\n        for (size_t i = 0; i < obs.size(); ++i)\n        {\n            const ObsPtr &this_obs = obs[i];\n            // check system\n            const uint32_t sys = satsys(this_obs->sat, NULL);\n            if (sys != SYS_GPS && sys != SYS_GLO && sys != SYS_BDS && sys != SYS_GAL)\n                continue;\n            // check signal frequency\n            const double obs_freq = L1_freq(this_obs, NULL);\n            if (obs_freq < 0)   continue;\n\n            L1_obs.push_back(this_obs);\n            L1_ephems.push_back(ephems[i]);\n        }\n    }\n\n    std::vector<SatStatePtr> sat_states(const std::vector<ObsPtr> &obs, \n        const std::vector<EphemBasePtr> &ephems)\n    {\n        std::vector<SatStatePtr> all_sv_states;\n        const uint32_t num_obs = obs.size();\n        for (size_t i = 0; i < num_obs; ++i)\n        {\n            SatStatePtr sat_state(new SatState());\n            all_sv_states.push_back(sat_state);\n\n            ObsPtr this_obs = obs[i];\n            const uint32_t sat = this_obs->sat;\n            const uint32_t sys = satsys(sat, NULL);\n            int l1_idx = -1;\n            L1_freq(this_obs, &l1_idx);\n            if (l1_idx < 0)   continue;\n            double tof = this_obs->psr[l1_idx] / LIGHT_SPEED;\n\n            gtime_t sv_tx = time_add(this_obs->time, -tof);\n            double svdt = 0, svddt = 0;\n            Eigen::Vector3d sv_pos = Eigen::Vector3d::Zero();\n            Eigen::Vector3d sv_vel = Eigen::Vector3d::Zero();\n            if (sys == SYS_GLO)\n            {\n                GloEphemPtr glo_ephem = std::dynamic_pointer_cast<GloEphem>(ephems[i]);\n                svdt = geph2svdt(sv_tx, glo_ephem);\n                sv_tx = time_add(sv_tx, -svdt);\n                sv_pos = geph2pos(sv_tx, glo_ephem, &svdt);\n                sv_vel = geph2vel(sv_tx, glo_ephem, &svddt);\n            }\n            else\n            {\n                EphemPtr ephem = std::dynamic_pointer_cast<Ephem>(ephems[i]);\n                svdt = eph2svdt(sv_tx, ephem);\n                sv_tx = time_add(sv_tx, -svdt);\n                sv_pos = eph2pos(sv_tx, ephem, &svdt);\n                sv_vel = eph2vel(sv_tx, ephem, &svddt);\n                sat_state->tgd = ephem->tgd[0];\n            }\n            sat_state->sat_id = sat;\n            sat_state->ttx    = sv_tx;\n            sat_state->pos    = sv_pos;\n            sat_state->vel    = sv_vel;\n            sat_state->dt     = svdt;\n            sat_state->ddt    = svddt;\n        }\n        return all_sv_states;\n    }\n\n    void psr_res(const Eigen::Matrix<double, 7, 1> &rcv_state, const std::vector<ObsPtr> &obs, \n        const std::vector<SatStatePtr> &all_sv_states, const std::vector<double> &iono_params, \n        Eigen::VectorXd &res, Eigen::MatrixXd &J, std::vector<Eigen::Vector2d> &atmos_delay, \n        std::vector<Eigen::Vector2d> &all_sv_azel)\n    {\n        const uint32_t num_sv = all_sv_states.size();\n        // clear output\n        res = Eigen::VectorXd::Zero(num_sv);\n        J = Eigen::MatrixXd::Zero(num_sv, 7);\n        atmos_delay.resize(num_sv);\n        all_sv_azel.resize(num_sv);\n\n        for (uint32_t i = 0; i < num_sv; ++i)\n        {\n            int l1_idx = -1;\n            L1_freq(obs[i], &l1_idx);\n            if (l1_idx < 0)   continue;\n\n            const SatStatePtr &sat_state = all_sv_states[i];\n            uint32_t this_sys = satsys(sat_state->sat_id, NULL);\n            Eigen::Vector3d sv_pos = sat_state->pos;\n\n            double ion_delay=0, tro_delay=0;\n            double azel[2] = {0, M_PI/2.0};\n            if (rcv_state.topLeftCorner<3,1>().norm() > 0)\n            {\n                sat_azel(rcv_state.topLeftCorner<3,1>(), sv_pos, azel);\n                Eigen::Vector3d rcv_lla = ecef2geo(rcv_state.head<3>());\n                // use satellite signal transmit time instead\n                tro_delay = calculate_trop_delay(sat_state->ttx, rcv_lla, azel);\n                ion_delay = calculate_ion_delay(sat_state->ttx, iono_params, rcv_lla, azel);\n            }\n\n            Eigen::Vector3d rv2sv = sv_pos - rcv_state.topLeftCorner<3,1>();\n            Eigen::Vector3d unit_rv2sv = rv2sv.normalized();\n            double sagnac_term = EARTH_OMG_GPS*(sv_pos(0)*rcv_state(1,0)-\n                sv_pos(1)*rcv_state(0,0))/LIGHT_SPEED;\n            double psr_estimated = rv2sv.norm() + sagnac_term + rcv_state(3+sys2idx.at(this_sys)) -\n             sat_state->dt*LIGHT_SPEED + tro_delay + ion_delay + sat_state->tgd*LIGHT_SPEED;\n\n            J.block(i, 0, 1, 3) = -unit_rv2sv.transpose();\n            J(i, 3+sys2idx.at(this_sys)) = 1.0;\n            res(i) = psr_estimated - obs[i]->psr[l1_idx];\n\n            atmos_delay[i] = Eigen::Vector2d(ion_delay, tro_delay);\n            all_sv_azel[i] = Eigen::Vector2d(azel[0], azel[1]);\n        }\n    }\n\n    Eigen::Matrix<double, 7, 1> psr_pos(const std::vector<ObsPtr> &obs, \n        const std::vector<EphemBasePtr> &ephems, const std::vector<double> &iono_params)\n    {\n        Eigen::Matrix<double, 7, 1> result;\n        result.setZero();\n\n        std::vector<ObsPtr> valid_obs;\n        std::vector<EphemBasePtr> valid_ephems;\n        filter_L1(obs, ephems, valid_obs, valid_ephems);\n        if (valid_obs.size() < 4)\n        {\n            LOG(ERROR) << \"[gnss_comm::psr_pos] GNSS observation not enough.\\n\";\n            return result;\n        }\n\n        std::vector<SatStatePtr> all_sat_states = sat_states(valid_obs, valid_ephems);\n\n        Eigen::Matrix<double, 7, 1> xyzt;\n        xyzt.setZero();\n        double dx_norm = 1.0;\n        uint32_t num_iter = 0;\n        while(num_iter < MAX_ITER_PVT && dx_norm > EPSILON_PVT)\n        {\n            Eigen::VectorXd b;\n            Eigen::MatrixXd G;\n            std::vector<Eigen::Vector2d> atmos_delay;\n            std::vector<Eigen::Vector2d> all_sv_azel;\n            psr_res(xyzt, valid_obs, all_sat_states, iono_params, b, G, atmos_delay, all_sv_azel);\n\n            std::vector<uint32_t> good_idx;\n            for (uint32_t i = 0; i < valid_obs.size(); ++i)\n            {\n                if (G.row(i).norm() <= 0)   continue;       // res not computed\n                if (all_sv_azel[i].y() > CUT_OFF_DEGREE/180.0*M_PI)  \n                    good_idx.push_back(i);\n            }\n            int sys_mask[4] = {0, 0, 0, 0};\n            for (uint32_t i = 0; i < valid_obs.size(); ++i)\n            {\n                const uint32_t obs_sys = satsys(valid_obs[i]->sat, NULL);\n                sys_mask[sys2idx.at(obs_sys)] = 1;\n            }\n            uint32_t num_extra_constraint = 4;\n            for (uint32_t k = 0; k < 4; ++k)    num_extra_constraint -= sys_mask[k];\n            LOG_IF(FATAL, num_extra_constraint >= 4) << \"[gnss_comm::psr_pos] too many extra-clock constraints.\\n\";\n\n            const uint32_t good_num = good_idx.size();\n            LOG_IF(ERROR, good_num < 4) << \"too few good obs: \" << good_num;\n\n            Eigen::MatrixXd good_G(good_num+num_extra_constraint, 7);\n            Eigen::VectorXd good_b(good_num+num_extra_constraint);\n            Eigen::MatrixXd good_W(good_num+num_extra_constraint, good_num+num_extra_constraint);\n            good_W.setZero();\n            for (uint32_t i = 0; i < good_num; ++i)\n            {\n                const uint32_t origin_idx = good_idx[i];\n                const ObsPtr &this_obs = valid_obs[origin_idx];\n                good_G.row(i) = G.row(origin_idx);\n                good_b(i) = b(origin_idx);\n                // compose weight\n                const double sin_el = sin(all_sv_azel[origin_idx].y());\n                double weight = sin_el*sin_el;\n                int l1_idx = -1;\n                L1_freq(this_obs, &l1_idx);\n                LOG_IF(FATAL, l1_idx < 0) << \"[gnss_comm::psr_pos] no L1 observation found.\\n\";\n\n                if (this_obs->psr_std[l1_idx] > 0)\n                    weight /= (this_obs->psr_std[l1_idx]/0.16);\n                const uint32_t obs_sys = satsys(this_obs->sat, NULL);\n                if (obs_sys == SYS_GPS || obs_sys == SYS_BDS)\n                    weight /= valid_ephems[origin_idx]->ura-1;\n                else if (obs_sys == SYS_GAL)\n                    weight /= valid_ephems[origin_idx]->ura-2;\n                else if (obs_sys == SYS_GLO)\n                    weight /= 4;\n                good_W(i, i) = weight;\n            }\n            uint32_t tmp_count = good_num;\n            // add extra pseudo measurement to contraint unobservable clock bias\n            for (size_t k = 0; k < 4; ++k)\n            {\n                if (!sys_mask[k])\n                {\n                    good_G.row(tmp_count).setZero();\n                    good_G(tmp_count, k+3) = 1.0;\n                    good_b(tmp_count) = 0;\n                    good_W(tmp_count, tmp_count) = 1000;       // large weight\n                    ++tmp_count;\n                }\n            }\n\n            // ready for solving\n            Eigen::VectorXd dx = -(good_G.transpose()*good_W*good_G).inverse() * good_G.transpose() * good_W * good_b;\n            dx_norm = dx.norm();\n            xyzt += dx;\n            // LOG(INFO) << \"cov is \\n\" << (G.transpose()*W*G).inverse();\n            ++num_iter;\n        }\n        if (num_iter == MAX_ITER_PVT)\n        {\n            LOG(WARNING) << \"[gnss_comm::psr_pos] XYZT solver reached maximum iterations.\\n\";\n            return result;\n        }\n\n        result = xyzt;\n        return result;\n    }\n\n    void dopp_res(const Eigen::Matrix<double, 4, 1> &rcv_state, const Eigen::Vector3d &rcv_ecef,\n                  const std::vector<ObsPtr> &obs, const std::vector<SatStatePtr> &all_sv_states, \n                  Eigen::VectorXd &res, Eigen::MatrixXd &J)\n    {\n        const uint32_t num_sv = all_sv_states.size();\n        //clear output\n        res = Eigen::VectorXd::Zero(num_sv);\n        J = Eigen::MatrixXd::Zero(num_sv, 4);\n\n        for (uint32_t i = 0; i < num_sv; ++i)\n        {\n            const SatStatePtr &sat_state = all_sv_states[i];\n            Eigen::Vector3d unit_rv2sv = (sat_state->pos - rcv_ecef).normalized();\n            double sagnac_term = EARTH_OMG_GPS/LIGHT_SPEED*(\n                sat_state->vel(0)*rcv_ecef(1)+ sat_state->pos(0)*rcv_state(1,0) - \n                sat_state->vel(1)*rcv_ecef(0) - sat_state->pos(1)*rcv_state(0,0));\n            double dopp_estimated = (sat_state->vel - rcv_state.topLeftCorner<3, 1>()).dot(unit_rv2sv) + \n                    rcv_state(3, 0) + sagnac_term - sat_state->ddt*LIGHT_SPEED;\n            int l1_idx = -1;\n            const double obs_freq = L1_freq(obs[i], &l1_idx);\n            if (obs_freq < 0)   continue;\n            const double wavelength = LIGHT_SPEED / obs_freq;\n            res(i) = dopp_estimated + obs[i]->dopp[l1_idx]*wavelength;\n            J.block(i, 0, 1, 3) = -1.0 * unit_rv2sv.transpose();\n            J(i, 3) = 1.0;\n        }\n    }\n\n    Eigen::Matrix<double, 4, 1> dopp_vel(const std::vector<ObsPtr> &obs, \n        const std::vector<EphemBasePtr> &ephems, Eigen::Vector3d &ref_ecef)\n    {\n        // LOG(INFO) << \"try to solve Doppler velocity.\";\n        Eigen::Matrix<double, 4, 1> result;\n        result.setZero();\n\n        if (ref_ecef.norm() == 0)\n        {\n            // reference point not given, try calculate using pseudorange\n            std::vector<double> zero_iono_params(8, 0.0);\n            Eigen::Matrix<double, 7, 1> psr_result = psr_pos(obs, ephems, zero_iono_params);\n            if (psr_result.head<3>().norm() != 0)\n            {\n                ref_ecef = psr_result.head<3>();\n            }\n            else\n            {\n                LOG(ERROR) << \"[gnss_comm::dopp_vel] Unable to initialize reference position for Doppler calculation.\\n\";\n                return result;\n            }\n        }\n\n        std::vector<ObsPtr> valid_obs;\n        std::vector<EphemBasePtr> valid_ephems;\n        filter_L1(obs, ephems, valid_obs, valid_ephems);\n\n        if (valid_obs.size() < 4)\n        {\n            LOG(ERROR) << \"[gnss_comm::dopp_vel] GNSS observation not enough for velocity calculation.\\n\";\n            return result;\n        }\n\n        std::vector<SatStatePtr> all_sat_states = sat_states(valid_obs, valid_ephems);\n        // compute azel for all satellite\n        std::vector<Eigen::Vector2d> all_sv_azel;\n        for (uint32_t i = 0; i < all_sat_states.size(); ++i)\n        {\n            double azel[2] = {0, 0};\n            sat_azel(ref_ecef, all_sat_states[i]->pos, azel);\n            all_sv_azel.emplace_back(azel[0], azel[1]);\n        }\n\n        Eigen::Matrix<double, 4, 1> xyzt_dot;\n        xyzt_dot.setZero();\n        double dx_norm = 1.0;\n        uint32_t num_iter = 0;\n        while(num_iter < MAX_ITER_PVT && dx_norm > EPSILON_PVT)\n        {\n            Eigen::MatrixXd G;\n            Eigen::VectorXd b;\n            dopp_res(xyzt_dot, ref_ecef, valid_obs, all_sat_states, b, G);\n            \n            std::vector<uint32_t> good_idx;\n            for (uint32_t i = 0; i < valid_obs.size(); ++i)\n            {\n                if (G.row(i).norm() <= 0)   continue;       // res not computed\n                if (all_sv_azel[i].y() > CUT_OFF_DEGREE/180.0*M_PI)  \n                    good_idx.push_back(i);\n            }\n            const uint32_t good_num = good_idx.size();\n            Eigen::MatrixXd good_G = Eigen::MatrixXd::Zero(good_num, 4);\n            Eigen::VectorXd good_b = Eigen::VectorXd::Zero(good_num);\n            Eigen::MatrixXd good_W = Eigen::MatrixXd::Zero(good_num, good_num);\n            for (uint32_t i = 0; i < good_num; ++i)\n            {\n                const uint32_t origin_idx = good_idx[i];\n                good_G.row(i) = G.row(origin_idx);\n                good_b(i) = b(origin_idx);\n                const double sin_el = sin(all_sv_azel[origin_idx].y());\n                double weight = sin_el*sin_el;\n                const ObsPtr &this_obs = valid_obs[origin_idx];\n                int l1_idx = -1;\n                L1_freq(this_obs, &l1_idx);\n                LOG_IF(FATAL, l1_idx < 0) << \"[gnss_comm::dopp_vel] no L1 observation found.\\n\";\n                if (this_obs->dopp_std[l1_idx] > 0)\n                    weight /= (this_obs->dopp_std[l1_idx]/0.256);\n\n                const uint32_t obs_sys = satsys(this_obs->sat, NULL);\n                if (obs_sys == SYS_GPS || obs_sys == SYS_BDS)\n                    weight /= valid_ephems[origin_idx]->ura-1;\n                else if (obs_sys == SYS_GAL)\n                    weight /= valid_ephems[origin_idx]->ura-2;\n                else if (obs_sys == SYS_GLO)\n                    weight /= 2;\n                good_W(i, i) = weight;\n            }\n\n            // solve\n            Eigen::VectorXd dx = -(good_G.transpose()*good_W*good_G).inverse() * good_G.transpose() * good_W * good_b;\n            dx_norm = dx.norm();\n            xyzt_dot += dx;\n\n            // dx_norm = 0;\n            // LOG(INFO) << \"cov is \\n\" << (G.transpose()*W*G).inverse();\n            ++num_iter;\n        }\n\n        if (num_iter == MAX_ITER_PVT)\n            LOG(WARNING) << \"[gnss_comm::dopp_vel] XYZT solver reached maximum iterations.\\n\";\n        \n        result = xyzt_dot;\n        return result;\n    }\n}\n"
  },
  {
    "path": "gnss_comm/src/gnss_utility.cpp",
    "content": "/**\n* This file is part of gnss_comm.\n*\n* Copyright (C) 2021 Aerial Robotics Group, Hong Kong University of Science and Technology\n* Author: CAO Shaozu (shaozu.cao@gmail.com)\n*\n* gnss_comm is free software: you can redistribute it and/or modify\n* it under the terms of the GNU General Public License as published by\n* the Free Software Foundation, either version 3 of the License, or\n* (at your option) any later version.\n*\n* gnss_comm is distributed in the hope that it will be useful,\n* but WITHOUT ANY WARRANTY; without even the implied warranty of\n* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n* GNU General Public License for more details.\n*\n* You should have received a copy of the GNU General Public License\n* along with gnss_comm. If not, see <http://www.gnu.org/licenses/>.\n*\n* As many of the utility functions are adapted from RTKLIB, \n* the license for those part of code is claimed as follows:\n* \n* The RTKLIB software package is distributed under the following BSD 2-clause\n* license (http://opensource.org/licenses/BSD-2-Clause) and additional two\n* exclusive clauses. Users are permitted to develop, produce or sell their own\n* non-commercial or commercial products utilizing, linking or including RTKLIB as\n* long as they comply with the license.\n* \n*         Copyright (c) 2007-2020, T. Takasu, All rights reserved.\n*/\n\n#include <gnss_comm/gnss_utility.hpp>\n\nnamespace gnss_comm\n{\n    // some of the following functions are adapted from RTKLIB\n\n    const static double gpst0[] = {1980,1,6,0,0,0}; /* gps time reference */\n    const static double gst0 [] = {1999,8,22,0,0,0}; /* galileo system time reference */\n    const static double bdt0 [] = {2006,1,1,0,0,0}; /* beidou time reference */\n\n    /* satellite system+prn/slot number to satellite number ------------------------\n    * convert satellite system+prn/slot number to satellite number\n    * args   : uint32_t    sys       I   satellite system (SYS_GPS,SYS_GLO,...)\n    *          uint32_t    prn       I   satellite prn/slot number\n    * return : satellite number (0:error)\n    *-----------------------------------------------------------------------------*/\n    uint32_t sat_no(uint32_t sys, uint32_t prn)\n    {\n        if (prn == 0) return 0;\n        switch (sys) {\n            case SYS_GPS:\n                if (prn < MIN_PRN_GPS || prn > MAX_PRN_GPS) return 0;\n                return prn-MIN_PRN_GPS+1;\n            case SYS_GLO:\n                if (prn < MIN_PRN_GLO || prn > MAX_PRN_GLO) return 0;\n                return N_SAT_GPS+prn-MIN_PRN_GLO+1;\n            case SYS_GAL:\n                if (prn < MIN_PRN_GAL || prn > MAX_PRN_GAL) return 0;\n                return N_SAT_GPS+N_SAT_GLO+prn-MIN_PRN_GAL+1;\n            case SYS_BDS:\n                if (prn < MIN_PRN_BDS || prn > MAX_PRN_BDS) return 0;\n                return N_SAT_GPS+N_SAT_GLO+N_SAT_GAL+prn-MIN_PRN_BDS+1;\n        }\n        return 0;\n    }\n\n    /* satellite number to satellite system ----------------------------------------\n    * convert satellite number to satellite system\n    * args   : uint32_t    sat       I   satellite number (1-MAXSAT)\n    *          uint32_t    *prn      IO  satellite prn/slot number (NULL: no output)\n    * return : satellite system (SYS_GPS,SYS_GLO,...)\n    *-----------------------------------------------------------------------------*/\n    uint32_t satsys(uint32_t sat, uint32_t *prn)\n    {\n        uint32_t sys = SYS_NONE;\n        if (sat <= 0 || sat > MAX_SAT) sat = 0;\n        else if (sat <= N_SAT_GPS) {\n            sys = SYS_GPS; sat += MIN_PRN_GPS-1;\n        }\n        else if ((sat-=N_SAT_GPS) <= N_SAT_GLO) {\n            sys = SYS_GLO; sat += MIN_PRN_GLO-1;\n        }\n        else if ((sat-=N_SAT_GLO) <= N_SAT_GAL) {\n            sys = SYS_GAL; sat += MIN_PRN_GAL-1;\n        }\n        else if ((sat-=N_SAT_GAL) <= N_SAT_BDS) {\n            sys = SYS_BDS; sat += MIN_PRN_BDS-1; \n        }\n        else sat = 0;\n        if (prn) *prn = sat;\n        return sys;\n    }\n\n    /* convert calendar day/time to time -------------------------------------------\n    * convert calendar day/time to gtime_t struct\n    * args   : double *ep       I   day/time {year,month,day,hour,min,sec}\n    * return : gtime_t struct\n    * notes  : proper in 1970-2037 or 1970-2099 (64bit time_t)\n    *-----------------------------------------------------------------------------*/\n    gtime_t epoch2time(const double *ep)\n    {\n        const int doy[] = {1,32,60,91,121,152,182,213,244,274,305,335};\n        gtime_t time = {0};\n        int days, sec, year=(int)ep[0], mon=(int)ep[1], day=(int)ep[2];\n        \n        if (year < 1970 || year > 2099 || mon < 1 || mon > 12) return time;\n        \n        /* leap year if year%4==0 in 1901-2099 */\n        days = (year-1970)*365 + (year-1969)/4 + doy[mon-1] + day-2 + (year%4==0&&mon>=3?1:0);\n        sec = (int)floor(ep[5]);\n        time.time = (time_t)days*86400 + (int)ep[3]*3600 + (int)ep[4]*60 + sec;\n        time.sec = ep[5] - sec;\n        return time;\n    }\n    /* time to calendar day/time ---------------------------------------------------\n    * convert gtime_t struct to calendar day/time\n    * args   : gtime_t t        I   gtime_t struct\n    *          double *ep       O   day/time {year,month,day,hour,min,sec}\n    * return : none\n    * notes  : proper in 1970-2037 or 1970-2099 (64bit time_t)\n    *-----------------------------------------------------------------------------*/\n    void time2epoch(gtime_t t, double *ep)\n    {\n        const int mday[] = { /* # of days in a month */\n            31,28,31,30,31,30,31,31,30,31,30,31,31,28,31,30,31,30,31,31,30,31,30,31,\n            31,29,31,30,31,30,31,31,30,31,30,31,31,28,31,30,31,30,31,31,30,31,30,31\n        };\n        int days, sec, mon, day;\n        \n        /* leap year if year%4==0 in 1901-2099 */\n        days = (int)(t.time / 86400);\n        sec = (int)(t.time - (time_t)days*86400);\n        for (day=days%1461, mon=0; mon<48; mon++) {\n            if (day >= mday[mon]) day -= mday[mon]; else break;\n        }\n        ep[0] = 1970 + days/1461*4 + mon/12;\n        ep[1] = mon%12 + 1;\n        ep[2] = day + 1;\n        ep[3] = sec / 3600;\n        ep[4] = sec%3600 / 60;\n        ep[5] = sec%60 + t.sec;\n    }\n\n    /* gps time to time ------------------------------------------------------------\n    * convert week and tow in gps time to gtime_t struct\n    * args   : uint32_t    week      I   week number in gps time\n    *          double      tow       I   time of week in gps time (s)\n    * return : gtime_t struct\n    *-----------------------------------------------------------------------------*/\n    gtime_t gpst2time(uint32_t week, double tow)\n    {\n        gtime_t t = epoch2time(gpst0);\n        if (tow < -1E9 || tow > 1E9) tow = 0.0;\n        t.time += 86400*7*week + (int)tow;\n        t.sec = tow - (int)tow;\n        return t;\n    }\n    /* time to gps time ------------------------------------------------------------\n    * convert gtime_t struct to week and tow in gps time\n    * args   : gtime_t      t        I   gtime_t struct\n    *          uint32_t    *week     IO  week number in gps time (NULL: no output)\n    * return : time of week in gps time (s)\n    *-----------------------------------------------------------------------------*/\n    double time2gpst(gtime_t t, uint32_t *week)\n    {\n        gtime_t t0 = epoch2time(gpst0);\n        time_t sec = t.time - t0.time;\n        uint32_t w = (uint32_t)(sec / (86400*7));\n        \n        if (week) *week = w;\n        return (double)(sec - w*86400*7) + t.sec;\n    }\n    /* galileo system time to time -------------------------------------------------\n    * convert week and tow in galileo system time (gst) to gtime_t struct\n    * args   : int    week      I   week number in gst\n    *          double tow       I   time of week in gst (s)\n    * return : gtime_t struct\n    *-----------------------------------------------------------------------------*/\n    gtime_t gst2time(int week, double tow)\n    {\n        gtime_t t = epoch2time(gst0);\n        \n        if (tow < -1E9 || tow > 1E9) tow = 0.0;\n        t.time += 86400*7*week + (int)tow;\n        t.sec = tow - (int)tow;\n        return t;\n    }\n    /* time to galileo system time -------------------------------------------------\n    * convert gtime_t struct to week and tow in galileo system time (gst)\n    * args   : gtime_t t        I   gtime_t struct\n    *          int    *week     IO  week number in gst (NULL: no output)\n    * return : time of week in gst (s)\n    *-----------------------------------------------------------------------------*/\n    double time2gst(gtime_t t, int *week)\n    {\n        gtime_t t0 = epoch2time(gst0);\n        time_t sec = t.time-t0.time;\n        int w = (int)(sec / (86400*7));\n        \n        if (week) *week = w;\n        return (double)(sec - w*86400*7) + t.sec;\n    }\n    /* beidou time (bdt) to time ---------------------------------------------------\n    * convert week and tow in beidou time (bdt) to gtime_t struct\n    * args   : int    week      I   week number in bdt\n    *          double tow       I   time of week in bdt (s)\n    * return : gtime_t struct\n    *-----------------------------------------------------------------------------*/\n    gtime_t bdt2time(int week, double tow)\n    {\n        gtime_t t = epoch2time(bdt0);\n        \n        if (tow < -1E9 || tow > 1E9) tow = 0.0;\n        t.time += 86400*7*week + (int)tow;\n        t.sec = tow - (int)tow;\n        return t;\n    }\n    /* time to beidouo time (bdt) --------------------------------------------------\n    * convert gtime_t struct to week and tow in beidou time (bdt)\n    * args   : gtime_t t        I   gtime_t struct\n    *          int    *week     IO  week number in bdt (NULL: no output)\n    * return : time of week in bdt (s)\n    *-----------------------------------------------------------------------------*/\n    double time2bdt(gtime_t t, int *week)\n    {\n        gtime_t t0 = epoch2time(bdt0);\n        time_t sec = t.time - t0.time;\n        int w = (int)(sec / (86400*7));\n        \n        if (week) *week = w;\n        return (double)(sec - w*86400*7) + t.sec;\n    }\n\n    /* gpstime to utc --------------------------------------------------------------\n    * convert gpstime to utc considering leap seconds\n    * args   : gtime_t t        I   time expressed in gpstime\n    * return : time expressed in utc\n    * notes  : ignore slight time offset under 100 ns\n    *-----------------------------------------------------------------------------*/\n    gtime_t gpst2utc(gtime_t t)\n    {\n        gtime_t tu;\n        int i;\n        \n        for (i=0;leaps[i][0]>0;i++) \n        {\n            tu=time_add(t,leaps[i][6]);\n            if (time_diff(tu,epoch2time(leaps[i]))>=0.0) return tu;\n        }\n        return t;\n    }\n\n    /* utc to gpstime --------------------------------------------------------------\n    * convert utc to gpstime considering leap seconds\n    * args   : gtime_t t        I   time expressed in utc\n    * return : time expressed in gpstime\n    * notes  : ignore slight time offset under 100 ns\n    *-----------------------------------------------------------------------------*/\n    gtime_t utc2gpst(gtime_t t)\n    {\n        int i;\n        \n        for (i=0;leaps[i][0]>0;i++) \n        {\n            if (time_diff(t,epoch2time(leaps[i]))>=0.0) return time_add(t,-leaps[i][6]);\n        }\n        return t;\n    }\n\n    double julian_day(std::vector<double> datetime)\n    {\n        LOG_IF(FATAL, datetime.size() != 6) << \"datetime should contain 6 fields\";\n        LOG_IF(FATAL, datetime[0] < 1901 || datetime[0] > 2099) << datetime[0] << \" should within range 1900 ~ 2100\";\n\n        if (datetime[1] <= 2)\n        {\n            datetime[1] += 12;\n            datetime[0] -= 1;\n        }\n        double julian_day = floor(365.25*datetime[0]) + floor(30.6001*(datetime[1]+1)) - \n                            15 + 1720996.5 + datetime[2] + datetime[3] / 24 + \n                            datetime[4] / 60 / 24 + datetime[5] / 3600 / 24;\n        return julian_day;\n    }\n\n    uint32_t leap_seconds_from_GPS_epoch(std::vector<double> datetime)\n    {\n        gtime_t given_t = epoch2time(&(datetime[0]));\n        for (size_t i = 0; leaps[i][0] > 0; ++i)\n        {\n            gtime_t tu = time_add(given_t, leaps[i][6]);\n            if (time_diff(tu, epoch2time(leaps[i])) >= 0.0)\n                return static_cast<uint32_t>(-leaps[i][6]);\n        }\n        return static_cast<uint32_t>(-1);\n    }\n\n    double time2doy(gtime_t time)\n    {\n        double ep[6];\n        time2epoch(time, ep);\n        ep[1] = ep[2] = 1.0;\n        ep[3] = ep[4] = ep[5] = 0.0;\n        return time_diff(time, epoch2time(ep))/86400.0 + 1.0;\n    }\n\n    double time_diff(gtime_t t1, gtime_t t2)\n    {\n        return difftime(t1.time,t2.time)+t1.sec-t2.sec;\n    }\n\n    gtime_t time_add(gtime_t t, double sec)\n    {\n        t.sec += sec;\n        double tt = floor(t.sec);\n        t.time += static_cast<int>(tt);\n        t.sec -= tt;\n        return t;\n    }\n\n    double time2sec(gtime_t time)\n    {\n        return static_cast<double>(time.time) + time.sec;\n    }\n\n    gtime_t sec2time(const double sec)\n    {\n        gtime_t time;\n        time.time = floor(sec);\n        time.sec = sec - time.time;\n        return time;\n    }\n\n    Eigen::Vector3d geo2ecef(const Eigen::Vector3d &lla)\n    {\n        Eigen::Vector3d xyz;\n        const double cos_lat = std::cos(lla(0)*D2R);\n        const double sin_lat = std::sin(lla(0)*D2R);\n        const double N = EARTH_SEMI_MAJOR / std::sqrt(1 - EARTH_ECCE_2*sin_lat*sin_lat);\n        xyz.x() = (N + lla(2)) * cos_lat * std::cos(lla(1)*D2R);\n        xyz.y() = (N + lla(2)) * cos_lat * std::sin(lla(1)*D2R);\n        xyz.z() = (N*(1-EARTH_ECCE_2) + lla(2)) * sin_lat;\n        return xyz;\n    }\n\n    Eigen::Vector3d ecef2geo(const Eigen::Vector3d &xyz)\n    {\n        Eigen::Vector3d lla = Eigen::Vector3d::Zero();\n        if (xyz.x() == 0 && xyz.y() == 0)\n        {\n            LOG(ERROR) << \"LLA coordinate is not defined if x = 0 and y = 0\";\n            return lla;\n        }\n\n        double e2 = EARTH_ECCE_2;\n        double a = EARTH_SEMI_MAJOR;\n        double a2 = a * a;\n        double b2 = a2 * (1 - e2);\n        double b = sqrt(b2);\n        double ep2 = (a2 - b2) / b2;\n        double p = xyz.head<2>().norm();\n\n        // two sides and hypotenuse of right angle triangle with one angle = theta:\n        double s1 = xyz.z() * a;\n        double s2 = p * b;\n        double h = sqrt(s1 * s1 + s2 * s2);\n        double sin_theta = s1 / h;\n        double cos_theta = s2 / h;\n\n        // two sides and hypotenuse of right angle triangle with one angle = lat:\n        s1 = xyz.z() + ep2 * b * pow(sin_theta, 3);\n        s2 = p - a * e2 * pow(cos_theta, 3);\n        h = sqrt(s1 * s1 + s2 * s2);\n        double tan_lat = s1 / s2;\n        double sin_lat = s1 / h;\n        double cos_lat = s2 / h;\n        double lat = atan(tan_lat);\n        double lat_deg = lat * R2D;\n\n        double N = a2 * pow((a2 * cos_lat * cos_lat + b2 * sin_lat * sin_lat), -0.5);\n        double altM = p / cos_lat - N;\n\n        double lon = atan2(xyz.y(), xyz.x());\n        double lon_deg = lon * R2D;\n        lla << lat_deg, lon_deg, altM;\n        return lla;\n    }\n\n    double Kepler(const double mk, const double es)\n    {\n        double e = mk;\n        double ek = 1e6;\n        size_t num_iter = 0;\n        while (num_iter < MAX_ITER_KEPLER && fabs(e-ek) > EPSILON_KEPLER)\n        {\n            ek = e;\n            e -= (e - es * sin(e) - mk) / (1.0 - es * cos(e));\n            ++ num_iter;\n        }\n        if (num_iter == MAX_ITER_KEPLER)\n            LOG(WARNING) << \"Kepler equation reached max number iteration\";\n        \n        return ek;\n    }\n\n    /* double eph2svdt(const gtime_t &curr_time, const EphemPtr ephem_ptr)\n    {\n        double tk = time_diff(curr_time, ephem_ptr->toe);\n        if (tk > WEEK_SECONDS/2)\n            tk -= WEEK_SECONDS;\n        else if (tk < -WEEK_SECONDS/2)\n            tk += WEEK_SECONDS;\n\n        double n0 = sqrt(MU / pow(ephem_ptr->A, 3));     // mean motion angular velocity (rad/sec)\n        double n = n0 + ephem_ptr->delta_n;              // corrected mean motion\n        double Mk = ephem_ptr->M0 + n * tk;\n        double Ek = Kepler(Mk, ephem_ptr->e);                     // solve Kepler equation for eccentric anomaly\n\n        double dt = time_diff(curr_time, ephem_ptr->toc);\n        if (dt > WEEK_SECONDS/2)\n            dt -= WEEK_SECONDS;\n        else if (dt < -WEEK_SECONDS/2)\n            dt += WEEK_SECONDS;\n        \n        for (size_t i = 0; i < 2; ++i)\n            dt -= ephem_ptr->af0 + ephem_ptr->af1*dt + ephem_ptr->af2*dt*dt;\n        \n        double dtsv = ephem_ptr->af0 + ephem_ptr->af1 * dt + ephem_ptr->af2 * dt * dt;\n        double rel_dt = -2.0 * sqrt(MU)/LIGHT_SPEED/LIGHT_SPEED * ephem_ptr->e * sqrt(ephem_ptr->A) * sin(Ek);\n        rel_dt = 0;\n        dtsv += rel_dt;\n        // LOG(INFO) << \"dtsv is \" << std::setprecision(10) << dtsv;\n        return dtsv;\n    } */\n\n    double eph2svdt(const gtime_t &curr_time, const EphemPtr ephem_ptr)\n    {\n        double dt = time_diff(curr_time, ephem_ptr->toc);\n        \n        for (size_t i = 0; i < 2; ++i)\n            dt -= ephem_ptr->af0 + ephem_ptr->af1*dt + ephem_ptr->af2*dt*dt;\n        \n        double dtsv = ephem_ptr->af0 + ephem_ptr->af1 * dt + ephem_ptr->af2 * dt * dt;\n        return dtsv;\n    }\n\n    Eigen::Vector3d eph2pos(const gtime_t &curr_time, const EphemPtr ephem_ptr, double *svdt)\n    {\n        Eigen::Vector3d sv_pos;\n        double tk = time_diff(curr_time, ephem_ptr->toe);\n\n        if (tk > WEEK_SECONDS/2)\n            tk -= WEEK_SECONDS;\n        else if (tk < -WEEK_SECONDS/2)\n            tk += WEEK_SECONDS;\n        \n        // LOG(INFO) << \"tk is \" << std::setprecision(10) << tk;\n        \n        if (std::abs(tk) > EPH_VALID_SECONDS)\n            LOG(WARNING) << \"Ephemeris is not valid anymore\";\n        \n        uint32_t prn;\n        uint32_t sys = satsys(ephem_ptr->sat, &prn);\n        double mu = MU, earth_omg = EARTH_OMG_GPS;\n        switch (sys)\n        {\n            case SYS_GPS: mu = MU_GPS; earth_omg = EARTH_OMG_GPS; break;\n            case SYS_GAL: mu = MU;     earth_omg = EARTH_OMG_GPS; break;\n            case SYS_GLO: mu = MU;     earth_omg = EARTH_OMG_GLO; break;\n            case SYS_BDS: mu = MU;     earth_omg = EARTH_OMG_BDS; break;\n        }\n\n        // LOG(INFO) << \"SYS: \" << sys << \", prn: \" << prn << \", curr_time is \" \n        //           << std::setprecision(10) << curr_time.time\n        //           << \", and toe is \" << ephem_ptr->toe.time;\n\n        double n0 = sqrt(mu / pow(ephem_ptr->A, 3));     // mean motion angular velocity (rad/sec)\n        double n = n0 + ephem_ptr->delta_n;              // corrected mean motion\n        double Mk = ephem_ptr->M0 + n * tk;\n        double Ek = Kepler(Mk, ephem_ptr->e);                     // solve Kepler equation for eccentric anomaly\n        double sin_Ek = sin(Ek), cos_Ek = cos(Ek);\n        double vk = atan2(sqrt(1 - ephem_ptr->e*ephem_ptr->e) * sin_Ek, cos_Ek - ephem_ptr->e);\n        double phi = vk + ephem_ptr->omg;\n        double cos_2phi = cos(2 * phi);\n        double sin_2phi = sin(2 * phi);\n\n        double delta_uk = ephem_ptr->cus * sin_2phi + ephem_ptr->cuc * cos_2phi;  // latitude correction\n        double delta_rk = ephem_ptr->crs * sin_2phi + ephem_ptr->crc * cos_2phi;  // radius correction\n        double delta_ik = ephem_ptr->cis * sin_2phi + ephem_ptr->cic * cos_2phi;  // correction to inclination\n        double uk = phi + delta_uk;                                             // corrected latitude\n        double rk = ephem_ptr->A * (1 - ephem_ptr->e * cos_Ek) + delta_rk;                // corrected radius\n        double ik = ephem_ptr->i0 + ephem_ptr->i_dot * tk + delta_ik;             // corrected inclination\n        double sin_ik = sin(ik), cos_ik = cos(ik);\n\n        double xk_prime = rk * cos(uk);     // x position in orbital plane\n        double yk_prime = rk * sin(uk);     // y position in orbital plane\n\n        double toe_tow = (sys == SYS_BDS ? time2bdt(time_add(ephem_ptr->toe, -14), NULL) : ephem_ptr->toe_tow);\n\n        if (sys == SYS_BDS && prn <= 5)     // BDS GEO satellite\n        {\n            double OMG_k = ephem_ptr->OMG0 + ephem_ptr->OMG_dot * tk - earth_omg * toe_tow;\n            double sin_OMG_k = sin(OMG_k), cos_OMG_k = cos(OMG_k);\n            double xg = xk_prime * cos_OMG_k - yk_prime * cos_ik * sin_OMG_k;\n            double yg = xk_prime * sin_OMG_k + yk_prime * cos_ik * cos_OMG_k;\n            double zg = yk_prime * sin_ik;\n            double sin_o = sin(earth_omg * tk), cos_o = cos(earth_omg * tk);\n            sv_pos.x() =  xg * cos_o + yg * sin_o * COS_N5 + zg * sin_o * SIN_N5;\n            sv_pos.y() = -xg * sin_o + yg * cos_o * COS_N5 + zg * cos_o * SIN_N5;\n            sv_pos.z() = -yg * SIN_N5 + zg * COS_N5;\n        }\n        else\n        {\n            double OMG_k = ephem_ptr->OMG0 + (ephem_ptr->OMG_dot - earth_omg) * tk \n                    - earth_omg * toe_tow;\n            double sin_OMG_k = sin(OMG_k), cos_OMG_k = cos(OMG_k);\n            sv_pos.x() = xk_prime * cos_OMG_k - yk_prime * cos_ik * sin_OMG_k;\n            sv_pos.y() = xk_prime * sin_OMG_k + yk_prime * cos_ik * cos_OMG_k;\n            sv_pos.z() = yk_prime * sin_ik;\n        }\n\n        double dt = time_diff(curr_time, ephem_ptr->toc);\n        double dts = ephem_ptr->af0 + ephem_ptr->af1 * dt + ephem_ptr->af2 * dt * dt;\n        // relativity correction\n        dts -= 2.0 * sqrt(mu * ephem_ptr->A) * ephem_ptr->e * sin_Ek / LIGHT_SPEED / LIGHT_SPEED;\n\n        if (svdt)  *svdt = dts;\n        \n        return sv_pos;\n    }\n\n    Eigen::Vector3d eph2vel(const gtime_t &curr_time, const EphemPtr ephem_ptr, double *svddt)\n    {\n        Eigen::Vector3d sv_vel;\n        sv_vel.setZero();\n        double tk = time_diff(curr_time, ephem_ptr->toe);\n\n        if (tk > WEEK_SECONDS/2)\n            tk -= WEEK_SECONDS;\n        else if (tk < -WEEK_SECONDS/2)\n            tk += WEEK_SECONDS;\n        \n        // LOG(INFO) << \"tk is \" << std::setprecision(10) << tk;\n        \n        if (std::abs(tk) > EPH_VALID_SECONDS)\n            LOG(WARNING) << \"Ephemeris is not valid anymore\";\n        \n        uint32_t prn;\n        uint32_t sys = satsys(ephem_ptr->sat, &prn);\n        double mu = MU, earth_omg = EARTH_OMG_GPS;\n        switch (sys)\n        {\n            case SYS_GPS: mu = MU_GPS; earth_omg = EARTH_OMG_GPS; break;\n            case SYS_GAL: mu = MU;     earth_omg = EARTH_OMG_GPS; break;\n            case SYS_GLO: mu = MU;     earth_omg = EARTH_OMG_GLO; break;\n            case SYS_BDS: mu = MU;     earth_omg = EARTH_OMG_BDS; break;\n        }\n        double n0 = sqrt(mu / pow(ephem_ptr->A, 3));     // mean motion angular velocity (rad/sec)\n        double n = n0 + ephem_ptr->delta_n;              // corrected mean motion\n        double Mk = ephem_ptr->M0 + n * tk;\n        double Ek = Kepler(Mk, ephem_ptr->e);                     // solve Kepler equation for eccentric anomaly\n        double sin_Ek = sin(Ek), cos_Ek = cos(Ek);\n        double Ek_dot = n / (1 - ephem_ptr->e*cos_Ek);\n        double vk_dot = sqrt(1-ephem_ptr->e*ephem_ptr->e) * Ek_dot / (1 - ephem_ptr->e*cos_Ek);\n        double vk = atan2(sqrt(1 - ephem_ptr->e*ephem_ptr->e) * sin_Ek, cos_Ek - ephem_ptr->e);\n        double phi = vk + ephem_ptr->omg;\n        double cos_2phi = cos(2 * phi);\n        double sin_2phi = sin(2 * phi);\n\n        double delta_uk_dot = 2 * vk_dot * (ephem_ptr->cus*cos_2phi - ephem_ptr->cuc*sin_2phi);\n        double delta_rk_dot = 2 * vk_dot * (ephem_ptr->crs*cos_2phi - ephem_ptr->crc*sin_2phi);\n        double delta_ik_dot = 2 * vk_dot * (ephem_ptr->cis*cos_2phi - ephem_ptr->cic*sin_2phi);\n\n        double uk_dot   = vk_dot + delta_uk_dot;\n        double rk_dot   = ephem_ptr->A * ephem_ptr->e * Ek_dot * sin_Ek + delta_rk_dot;\n        double ik_dot   = ephem_ptr->i_dot + delta_ik_dot;\n\n\n        double delta_uk = ephem_ptr->cus * sin_2phi + ephem_ptr->cuc * cos_2phi;  // latitude correction\n        double delta_rk = ephem_ptr->crs * sin_2phi + ephem_ptr->crc * cos_2phi;  // radius correction\n        double delta_ik = ephem_ptr->cis * sin_2phi + ephem_ptr->cic * cos_2phi;  // correction to inclination\n        double uk = phi + delta_uk;                                     // corrected latitude\n        double rk = ephem_ptr->A * (1 - ephem_ptr->e * cos_Ek) + delta_rk;        // corrected radius\n        double ik = ephem_ptr->i0 + ephem_ptr->i_dot * tk + delta_ik;             // corrected inclination\n        double sin_ik = sin(ik), cos_ik = cos(ik);\n\n        double sin_uk = sin(uk), cos_uk = cos(uk);\n        double xk_prime = rk * cos(uk);     // x position in orbital plane\n        double yk_prime = rk * sin(uk);     // y position in orbital plane\n        double xk_prime_dot = rk_dot * cos_uk - rk * uk_dot * sin_uk;\n        double yk_prime_dot = rk_dot * sin_uk + rk * uk_dot * cos_uk;\n        \n\n        double toe_tow = (sys == SYS_BDS ? time2bdt(time_add(ephem_ptr->toe, -14), NULL) : ephem_ptr->toe_tow);\n\n        if (sys == SYS_BDS && prn <= 5)     // BDS GEO satellite\n        {\n            double OMG_k = ephem_ptr->OMG0 + ephem_ptr->OMG_dot * tk - earth_omg * toe_tow;\n            double sin_OMG_k = sin(OMG_k), cos_OMG_k = cos(OMG_k);\n            double OMGk_dot = ephem_ptr->OMG_dot;\n            double term1 = xk_prime_dot - yk_prime*OMGk_dot*cos_ik;\n            double term2 = xk_prime*OMGk_dot + yk_prime_dot*cos_ik-yk_prime*ik_dot*sin_ik;\n            double xg = xk_prime * cos_OMG_k - yk_prime * cos_ik * sin_OMG_k;\n            double yg = xk_prime * sin_OMG_k + yk_prime * cos_ik * cos_OMG_k;\n            double zg = yk_prime * sin_ik;\n            double xg_dot = term1 * cos_OMG_k - term2 * sin_OMG_k;\n            double yg_dot = term1 * sin_OMG_k + term2 * cos_OMG_k;\n            double zg_dot = yk_prime_dot * sin_ik + yk_prime_dot * ik_dot * cos_ik;\n            double sin_o = sin(earth_omg * tk), cos_o = cos(earth_omg * tk);\n            double sin_o_dot = earth_omg * cos_o, cos_o_dot = -earth_omg * sin_o;\n\n            sv_vel.x() = xg_dot*cos_o + xg*cos_o_dot + yg_dot*sin_o*COS_N5 + yg*sin_o_dot*COS_N5 + \n                         zg_dot*sin_o*SIN_N5 + zg*sin_o_dot*SIN_N5;\n            sv_vel.y() = -xg_dot*sin_o - xg*sin_o_dot + yg_dot*cos_o*COS_N5 + yg*cos_o_dot*COS_N5 + \n                         zg_dot*cos_o*SIN_N5 + zg*cos_o_dot*SIN_N5;\n            sv_vel.z() = -yg_dot*SIN_N5 + zg_dot*COS_N5;\n        }\n        else\n        {\n            double OMG_k = ephem_ptr->OMG0 + (ephem_ptr->OMG_dot - earth_omg) * tk \n                    - earth_omg * toe_tow;\n            double sin_OMG_k = sin(OMG_k), cos_OMG_k = cos(OMG_k);\n            double OMGk_dot = ephem_ptr->OMG_dot - earth_omg;\n            double term1 = xk_prime_dot - yk_prime*OMGk_dot*cos_ik;\n            double term2 = xk_prime*OMGk_dot + yk_prime_dot*cos_ik-yk_prime*ik_dot*sin_ik;\n            sv_vel.x() = term1 * cos_OMG_k - term2 * sin_OMG_k;\n            sv_vel.y() = term1 * sin_OMG_k + term2 * cos_OMG_k;\n            sv_vel.z() = yk_prime_dot * sin_ik + yk_prime_dot * ik_dot * cos_ik;\n        }\n\n        double dt = time_diff(curr_time, ephem_ptr->toc);\n        double ddts = ephem_ptr->af1 + 2.0 * ephem_ptr->af2 * dt;\n        // relativity correction\n        ddts -= 2.0 * sqrt(mu * ephem_ptr->A) * ephem_ptr->e * cos_Ek * Ek_dot / LIGHT_SPEED / LIGHT_SPEED;\n\n        if (svddt)  *svddt = ddts;\n        \n        return sv_vel;\n    }\n\n    void deq(const Eigen::Vector3d &pos, const Eigen::Vector3d &vel, const Eigen::Vector3d &acc,\n             Eigen::Vector3d &pos_dot, Eigen::Vector3d &vel_dot)\n    {\n        double r2 = pos.squaredNorm(), r3 = r2 * sqrt(r2);\n        double omg2 = EARTH_OMG_GLO * EARTH_OMG_GLO;\n        \n        if (r2 <= 0.0) \n        {\n            pos_dot.setZero();\n            vel_dot.setZero();\n            return;\n        }\n        /* ref [2] A.3.1.2 with bug fix for xdot[4],xdot[5] */\n        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 */\n        double b = 5.0 * pos.z()*pos.z() / r2;                                  /* 5*z^2/r^2 */\n        double c = -MU / r3 - a * (1.0 - b);                                    /* -mu/r^3-a(1-b) */\n        pos_dot.x() = vel.x(); pos_dot.y() = vel.y(); pos_dot.z() = vel.z();\n        vel_dot.x() = (c + omg2) * pos.x() + 2.0 * EARTH_OMG_GLO * vel.y() + acc.x();\n        vel_dot.y() = (c + omg2) * pos.y() - 2.0 * EARTH_OMG_GLO * vel.x() + acc.y();\n        vel_dot.z() = (c - 2.0 * a) * pos.z() + acc.z();\n    }\n\n    void glo_orbit(double dt, Eigen::Vector3d &pos, Eigen::Vector3d &vel, const Eigen::Vector3d &acc)\n    {\n        Eigen::Vector3d pos_dot1, pos_dot2, pos_dot3, pos_dot4;\n        Eigen::Vector3d vel_dot1, vel_dot2, vel_dot3, vel_dot4;\n        Eigen::Vector3d next_pos, next_vel;\n\n        deq(pos, vel, acc, pos_dot1, vel_dot1);\n        next_pos = pos + 0.5 * pos_dot1 * dt;\n        next_vel = vel + 0.5 * vel_dot1 * dt;\n        deq(next_pos, next_vel, acc, pos_dot2, vel_dot2);\n        next_pos = pos + 0.5 * pos_dot2 * dt;\n        next_vel = vel + 0.5 * vel_dot2 * dt;\n        deq(next_pos, next_vel, acc, pos_dot3, vel_dot3);\n        next_pos = pos + pos_dot3 * dt;\n        next_vel = vel + vel_dot3 * dt;\n        deq(next_pos, next_vel, acc, pos_dot4, vel_dot4);\n        pos += (pos_dot1 + 2.0 * pos_dot2 + 2.0 * pos_dot3 + pos_dot4) * dt / 6.0;\n        vel += (vel_dot1 + 2.0 * vel_dot2 + 2.0 * vel_dot3 + vel_dot4) * dt / 6.0;\n    }\n\n    double geph2svdt(const gtime_t &curr_time, const GloEphemPtr geph_ptr)\n    {\n        double dt = time_diff(curr_time, geph_ptr->toe);\n        if (dt > WEEK_SECONDS/2)\n            dt -= WEEK_SECONDS;\n        else if (dt < -WEEK_SECONDS/2)\n            dt += WEEK_SECONDS;\n        \n        for (size_t i = 0; i < 2; ++i)\n            dt -= -geph_ptr->tau_n + geph_ptr->gamma * dt;\n        double svdt = -geph_ptr->tau_n + geph_ptr->gamma * dt;\n        return svdt;\n    }\n\n    Eigen::Vector3d geph2pos(const gtime_t &curr_time, const GloEphemPtr geph_ptr, double *svdt)\n    {\n        Eigen::Vector3d sv_pos, sv_vel, sv_acc;\n        sv_pos << geph_ptr->pos[0], geph_ptr->pos[1], geph_ptr->pos[2];\n        sv_vel << geph_ptr->vel[0], geph_ptr->vel[1], geph_ptr->vel[2];\n        sv_acc << geph_ptr->acc[0], geph_ptr->acc[1], geph_ptr->acc[2];\n        \n        double dt = time_diff(curr_time, geph_ptr->toe);\n        double dts = -geph_ptr->tau_n + geph_ptr->gamma * dt;\n        if (svdt)   *svdt = dts;\n        \n        for (double tt = dt<0.0?-TSTEP:TSTEP; fabs(dt) > 1e-9; dt -= tt) {\n            if (fabs(dt) < TSTEP) tt = dt;\n            glo_orbit(tt, sv_pos, sv_vel, sv_acc);\n        }\n        return sv_pos;\n    }\n\n    Eigen::Vector3d geph2vel(const gtime_t &curr_time, const GloEphemPtr geph_ptr, double *svddt)\n    {\n        Eigen::Vector3d sv_pos, sv_vel, sv_acc;\n        sv_pos << geph_ptr->pos[0], geph_ptr->pos[1], geph_ptr->pos[2];\n        sv_vel << geph_ptr->vel[0], geph_ptr->vel[1], geph_ptr->vel[2];\n        sv_acc << geph_ptr->acc[0], geph_ptr->acc[1], geph_ptr->acc[2];\n        \n        double dt = time_diff(curr_time, geph_ptr->toe);\n        if (svddt)   *svddt = geph_ptr->gamma;\n        \n        for (double tt = dt<0.0?-TSTEP:TSTEP; fabs(dt) > 1e-9; dt -= tt) {\n            if (fabs(dt) < TSTEP) tt = dt;\n            glo_orbit(tt, sv_pos, sv_vel, sv_acc);\n        }\n        return sv_vel;\n    }\n\n    Eigen::Vector3d ecef2enu(const Eigen::Vector3d &ref_lla, const Eigen::Vector3d &v_ecef)\n    {\n        double lat = ref_lla.x() * D2R, lon = ref_lla.y() * D2R;\n        double sin_lat = sin(lat), cos_lat = cos(lat);\n        double sin_lon = sin(lon), cos_lon = cos(lon);\n        Eigen::Matrix3d R_enu_ecef;\n        R_enu_ecef << -sin_lon,             cos_lon,         0,\n                    -sin_lat*cos_lon, -sin_lat*sin_lon, cos_lat,\n                     cos_lat*cos_lon,  cos_lat*sin_lon, sin_lat;\n        return (R_enu_ecef * v_ecef);\n    }\n\n    Eigen::Matrix3d geo2rotation(const Eigen::Vector3d &ref_geo)\n    {\n        double lat = ref_geo.x() * D2R, lon = ref_geo.y() * D2R;\n        double sin_lat = sin(lat), cos_lat = cos(lat);\n        double sin_lon = sin(lon), cos_lon = cos(lon);\n        Eigen::Matrix3d R_ecef_enu;\n        R_ecef_enu << -sin_lon, -sin_lat*cos_lon, cos_lat*cos_lon,\n                       cos_lon, -sin_lat*sin_lon, cos_lat*sin_lon,\n                       0      ,  cos_lat        , sin_lat;\n        return R_ecef_enu;\n    }\n\n    Eigen::Matrix3d ecef2rotation(const Eigen::Vector3d &ref_ecef)\n    {\n        return geo2rotation(ecef2geo(ref_ecef));\n    }\n\n    void sat_azel(const Eigen::Vector3d &rev_pos, const Eigen::Vector3d &sat_pos, double *azel)\n    {\n        if (!azel)  return;\n        Eigen::Vector3d rev_lla = ecef2geo(rev_pos);\n        Eigen::Vector3d rev2sat_ecef = (sat_pos - rev_pos).normalized();\n        Eigen::Vector3d rev2sat_enu = ecef2enu(rev_lla, rev2sat_ecef);\n        azel[0] = rev2sat_ecef.head<2>().norm() < 1e-12 ? 0.0 : atan2(rev2sat_enu.x(), rev2sat_enu.y());\n        azel[0] += (azel[0] < 0 ? 2*M_PI : 0);\n        azel[1] = asin(rev2sat_enu.z());\n    }\n    \n    /* latitude in degree */\n    static double interpc(const double coef[], double lat)\n    {\n        int i=(int)(lat/15.0);\n        if (i<1) return coef[0]; else if (i>4) return coef[4];\n        return coef[i-1]*(1.0-lat/15.0+i)+coef[i]*(lat/15.0-i);\n    }\n\n    /* elevation in radius */\n    static double mapf(double el, double a, double b, double c)\n    {\n        double sinel=sin(el);\n        return (1.0+a/(1.0+b/(1.0+c)))/(sinel+(a/(sinel+b/(sinel+c))));\n    }\n\n    /* calculate troposphere hydrostatic/wet mapping functions  ------------------------------\n    * calculate troposphere hydrostatic/wet mapping functions by Niell model\n    * ref: Global mapping functions for the atmosphere delay at radio wavelengths\n    * args   : gtime_t time                 I   time (gpst)\n    * args   : Eigen::Vector3d rev_lla      I   receiver position in geodetic coordinate\n    *          double*         azel         I   satellite azimuth/elevation angle in radius\n    *          double*         mapfh        IO  hydrostatic mapping coefficient\n    * return : delay expressed by light travel distance (m)\n    *-----------------------------------------------------------------------------*/\n    static void nmf(gtime_t time, const Eigen::Vector3d rev_lla, const double azel[], \n                    double *mapfh, double *mapfw)\n    {\n        /* hydro-ave-a,b,c, hydro-amp-a,b,c, wet-a,b,c at latitude 15,30,45,60,75 */\n        const double coef[][5]={\n            { 1.2769934E-3, 1.2683230E-3, 1.2465397E-3, 1.2196049E-3, 1.2045996E-3},\n            { 2.9153695E-3, 2.9152299E-3, 2.9288445E-3, 2.9022565E-3, 2.9024912E-3},\n            { 62.610505E-3, 62.837393E-3, 63.721774E-3, 63.824265E-3, 64.258455E-3},\n            \n            { 0.0000000E-0, 1.2709626E-5, 2.6523662E-5, 3.4000452E-5, 4.1202191E-5},\n            { 0.0000000E-0, 2.1414979E-5, 3.0160779E-5, 7.2562722E-5, 11.723375E-5},\n            { 0.0000000E-0, 9.0128400E-5, 4.3497037E-5, 84.795348E-5, 170.37206E-5},\n            \n            { 5.8021897E-4, 5.6794847E-4, 5.8118019E-4, 5.9727542E-4, 6.1641693E-4},\n            { 1.4275268E-3, 1.5138625E-3, 1.4572752E-3, 1.5007428E-3, 1.7599082E-3},\n            { 4.3472961E-2, 4.6729510E-2, 4.3908931E-2, 4.4626982E-2, 5.4736038E-2}\n        };\n        const double aht[]={ 2.53E-5, 5.49E-3, 1.14E-3}; /* height correction */\n        \n        double ah[3],aw[3],dm,el=azel[1],lat=rev_lla.x(),hgt=rev_lla.z();\n        int i;\n        \n        if (el<=0.0) {\n            if (mapfw) *mapfw=0.0;\n            if (mapfh) *mapfh=0.0;\n            return;\n        }\n        /* year from doy 28, added half a year for southern latitudes */\n        double y=(time2doy(time)-28.0)/365.25+(lat<0.0?0.5:0.0);\n        \n        double cosy=cos(2.0*M_PI*y);\n        lat=fabs(lat);\n        \n        for (i=0;i<3;i++) {\n            ah[i]=interpc(coef[i  ],lat)-interpc(coef[i+3],lat)*cosy;\n            aw[i]=interpc(coef[i+6],lat);\n        }\n        /* ellipsoidal height is used instead of height above sea level */\n        dm=(1.0/sin(el)-mapf(el,aht[0],aht[1],aht[2]))*hgt/1E3;\n        \n        if (mapfw) *mapfw=mapf(el,aw[0],aw[1],aw[2]);\n        if (mapfh) *mapfh=mapf(el,ah[0],ah[1],ah[2])+dm;\n    }\n\n    double calculate_trop_delay(gtime_t time, const Eigen::Vector3d &rev_lla, const double *azel)\n    {\n        // compute tropospheric delay by standard atmosphere and saastamoinen model\n        const double temp0=15.0; /* temparature at sea level */\n        const double humi =0.7;  /* relative humidity */ \n        \n        if (rev_lla.z()<-100.0||1E4<rev_lla.z()||azel[1]<=0) return 0.0;\n        \n        /* standard atmosphere */\n        double hgt=rev_lla.z()<0.0?0.0:rev_lla.z();\n        \n        double pres=1013.25*pow(1.0-2.2557E-5*hgt,5.2568);\n        double temp=temp0-6.5E-3*hgt+273.16;\n        double e=6.108*humi*exp((17.15*temp-4684.0)/(temp-38.45));\n        \n        /* saastamoninen model */\n        double zhd=0.0022768*pres/(1.0-0.00266*cos(2.0*rev_lla.x()*D2R)-0.00028*hgt/1E3);\n        double zwd=0.002277*(1255.0/temp+0.05)*e;\n\n        double mapfh = 0.0, mapfw = 0.0;\n        nmf(time, rev_lla, azel, &mapfh, &mapfw);\n        return (mapfh*zhd + mapfw*zwd);\n    }\n\n    double calculate_ion_delay(gtime_t t, const std::vector<double> &ion_parameters, \n                               const Eigen::Vector3d &rev_lla, const double *azel)\n    {\n        if (ion_parameters.empty())         return 0.0;\n        if (rev_lla.z()<-1E3||azel[1]<=0)   return 0.0;\n        \n        /* earth centered angle (semi-circle) */\n        double psi=0.0137/(azel[1]/M_PI+0.11)-0.022;\n        \n        /* subionospheric latitude/longitude (semi-circle) */\n        double phi=rev_lla.x()/180.0+psi*cos(azel[0]);\n        if      (phi> 0.416) phi= 0.416;\n        else if (phi<-0.416) phi=-0.416;\n        double lam=rev_lla.y()/180.0+psi*sin(azel[0])/cos(phi*M_PI);\n        \n        /* geomagnetic latitude (semi-circle) */\n        phi+=0.064*cos((lam-1.617)*M_PI);\n        \n        /* local time (s) */\n        uint32_t week = 0;\n        double tt=43200.0*lam+time2gpst(t,&week);\n        tt-=floor(tt/86400.0)*86400.0; /* 0<=tt<86400 */\n        \n        /* slant factor */\n        double f=1.0+16.0*pow(0.53-azel[1]/M_PI,3.0);\n        \n        /* ionospheric delay */\n        double amp=ion_parameters[0]+phi*(ion_parameters[1]+phi*(ion_parameters[2]+phi*ion_parameters[3]));\n        double per=ion_parameters[4]+phi*(ion_parameters[5]+phi*(ion_parameters[6]+phi*ion_parameters[7]));\n        amp=amp<    0.0?    0.0:amp;\n        per=per<72000.0?72000.0:per;\n        double x=2.0*M_PI*(tt-50400.0)/per;\n        \n        return LIGHT_SPEED*f*(fabs(x)<1.57?5E-9+amp*(1.0+x*x*(-0.5+x*x/24.0)):5E-9);\n    }\n\n    std::string sat2str(uint32_t sat_no)\n    {\n        std::stringstream ss;\n        uint32_t prn = 0;\n        uint32_t sys = satsys(sat_no, &prn);\n        switch (sys)\n        {\n            case SYS_GPS:\n                ss << \"G\" << std::setw(2) << std::setfill('0') << prn;\n                break;\n            case SYS_GLO:\n                ss << \"R\" << std::setw(2) << std::setfill('0') << prn;\n                break;\n            case SYS_BDS:\n                ss << \"C\" << std::setw(2) << std::setfill('0') << prn;\n                break;\n            case SYS_GAL:\n                ss << \"E\" << std::setw(2) << std::setfill('0') << prn;\n                break;\n            case SYS_SBS:\n                ss << \"S\" << std::setw(2) << std::setfill('0') << prn;\n                break;\n            case SYS_QZS:\n                ss << \"J\" << std::setw(2) << std::setfill('0') << prn;\n                break;\n            default:    // current not support other system\n                LOG(WARNING) << \"currently not support satelite system id \" << sys;\n                break;\n        }\n        return ss.str();\n    }\n\n    double L1_freq(const ObsPtr &obs, int *l1_idx)\n    {\n        const uint32_t sys = satsys(obs->sat, NULL);\n        double freq_min = -1.0;\n        double freq_max = -1.0;\n        if (sys == SYS_GPS || sys == SYS_GAL)\n            freq_min = freq_max = FREQ1;\n        else if (sys == SYS_BDS)\n            freq_min = freq_max = FREQ1_BDS;\n        else if (sys == SYS_GLO)\n        {\n            freq_min = FREQ1_GLO - 7 * DFRQ1_GLO;\n            freq_max = FREQ1_GLO + 6 * DFRQ1_GLO;\n        }\n        \n        if (l1_idx != NULL)\n            *l1_idx = -1;\n        \n        for (uint32_t i = 0; i < obs->freqs.size(); ++i)\n        {\n            if (obs->freqs[i] >= freq_min && obs->freqs[i] <= freq_max)\n            {\n                if (l1_idx != NULL)\n                    *l1_idx = static_cast<int>(i);\n                return obs->freqs[i];\n            }\n        }\n\n        return -1.0;\n    }\n\n    uint32_t str2sat(const std::string &sat_str)\n    {\n        if (sat_str.size() < 3)\n        {\n            LOG(ERROR) << \"Invalid RINEX satellite identifier \" << sat_str;\n            return 0;\n        }\n        const uint32_t prn = std::stoul(sat_str.substr(1, 2));\n        switch (sat_str.at(0))\n        {\n            case 'G':\n                return sat_no(SYS_GPS, prn);\n            case 'R':\n                return sat_no(SYS_GLO, prn);\n            case 'C':\n                return sat_no(SYS_BDS, prn);\n            case 'E':\n                return sat_no(SYS_GAL, prn);\n            case 'S':\n                return sat_no(SYS_SBS, prn);\n            case 'J':\n                return sat_no(SYS_QZS, prn);\n        }\n        return 0;\n    }\n\n    std::string exec(const std::string &cmd) \n    {\n        char buffer[128];\n        std::string result = \"\";\n        FILE* pipe = popen(cmd.c_str(), \"r\");\n        if (!pipe) throw std::runtime_error(\"popen() failed!\");\n        try \n        {\n            while (fgets(buffer, sizeof buffer, pipe) != NULL) \n            {\n                result += buffer;\n            }\n        } \n        catch (...) \n        {\n            pclose(pipe);\n            throw;\n        }\n        pclose(pipe);\n        return result;\n    }\n\n}   // namespace gnss_comm\n"
  },
  {
    "path": "gnss_comm/src/rinex_helper.cpp",
    "content": "/**\n* This file is part of gnss_comm.\n*\n* Copyright (C) 2021 Aerial Robotics Group, Hong Kong University of Science and Technology\n* Author: CAO Shaozu (shaozu.cao@gmail.com)\n*\n* gnss_comm is free software: you can redistribute it and/or modify\n* it under the terms of the GNU General Public License as published by\n* the Free Software Foundation, either version 3 of the License, or\n* (at your option) any later version.\n*\n* gnss_comm is distributed in the hope that it will be useful,\n* but WITHOUT ANY WARRANTY; without even the implied warranty of\n* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n* GNU General Public License for more details.\n*\n* You should have received a copy of the GNU General Public License\n* along with gnss_comm. If not, see <http://www.gnu.org/licenses/>.\n*/\n\n#include <gnss_comm/rinex_helper.hpp>\n\nnamespace gnss_comm\n{\n    /* convert string to double  ----------------------------------------------------------\n    * args   : std::string    num_str        I   input string\n    * return : double value inside the string\n    *-------------------------------------------------------------------------------------*/\n    static double str2double(const std::string &num_str)\n    {\n        // replace character 'D' with 'e'\n        size_t D_pos = num_str.find(\"D\");\n        std::string tmp_str = num_str;\n        if (D_pos != std::string::npos)\n            tmp_str = tmp_str.replace(D_pos, 1, \"e\");\n        return std::stod(tmp_str);\n    }\n\n    static GloEphemPtr rinex_line2glo_ephem(const std::vector<std::string> &ephem_lines, const uint32_t gpst_leap_seconds)\n    {\n        LOG_IF(FATAL, ephem_lines.size() != 4) << \"GLO ephemeris record should contain 8 lines\";\n        LOG_IF(FATAL, ephem_lines[0].at(0) != 'R') << \"Not a valid GLO ephemeris record\";\n        GloEphemPtr glo_ephem(new GloEphem());\n\n        uint32_t prn = static_cast<uint32_t>(std::stoi(ephem_lines[0].substr(1, 2)));\n        glo_ephem->sat = sat_no(SYS_GLO, prn);\n        double epoch[6];\n        epoch[0] = static_cast<double>(std::stoi(ephem_lines[0].substr(4, 4)));\n        epoch[1] = static_cast<double>(std::stoi(ephem_lines[0].substr(9, 2)));\n        epoch[2] = static_cast<double>(std::stoi(ephem_lines[0].substr(12, 2)));\n        epoch[3] = static_cast<double>(std::stoi(ephem_lines[0].substr(15, 2)));\n        epoch[4] = static_cast<double>(std::stoi(ephem_lines[0].substr(18, 2)));\n        epoch[5] = static_cast<double>(std::stoi(ephem_lines[0].substr(21, 2)));\n        glo_ephem->toe = epoch2time(epoch);\n        glo_ephem->toe.time += gpst_leap_seconds;\n        glo_ephem->tau_n = -1.0 * str2double(ephem_lines[0].substr(23, 19));\n        glo_ephem->gamma = str2double(ephem_lines[0].substr(42, 19));\n\n        // the second line\n        glo_ephem->pos[0] = str2double(ephem_lines[1].substr(4, 19)) * 1e3;\n        glo_ephem->vel[0] = str2double(ephem_lines[1].substr(23, 19)) * 1e3;\n        glo_ephem->acc[0] = str2double(ephem_lines[1].substr(42, 19)) * 1e3;\n        glo_ephem->health = static_cast<uint32_t>(str2double(ephem_lines[1].substr(61, 19)));\n\n        // the third line\n        glo_ephem->pos[1] = str2double(ephem_lines[2].substr(4, 19)) * 1e3;\n        glo_ephem->vel[1] = str2double(ephem_lines[2].substr(23, 19)) * 1e3;\n        glo_ephem->acc[1] = str2double(ephem_lines[2].substr(42, 19)) * 1e3;\n        glo_ephem->freqo  = static_cast<int>(str2double(ephem_lines[2].substr(61, 19)));\n\n        // the forth line\n        glo_ephem->pos[2] = str2double(ephem_lines[3].substr(4, 19)) * 1e3;\n        glo_ephem->vel[2] = str2double(ephem_lines[3].substr(23, 19)) * 1e3;\n        glo_ephem->acc[2] = str2double(ephem_lines[3].substr(42, 19)) * 1e3;\n        glo_ephem->age  = static_cast<uint32_t>(str2double(ephem_lines[3].substr(61, 19)));\n\n        return glo_ephem;\n    }\n\n    static EphemPtr rinex_line2ephem(const std::vector<std::string> &ephem_lines)\n    {\n        LOG_IF(FATAL, ephem_lines.size() != 8) << \"Ephemeris record should contain 8 lines\";\n        uint32_t sat_sys = SYS_NONE;\n        if      (ephem_lines[0].at(0) == 'G')    sat_sys = SYS_GPS;\n        else if (ephem_lines[0].at(0) == 'C')    sat_sys = SYS_BDS;\n        else if (ephem_lines[0].at(0) == 'E')    sat_sys = SYS_GAL;\n        LOG_IF(FATAL, sat_sys == SYS_NONE) << \"Satellite system is not supported: \" << ephem_lines[0].at(0);\n\n        EphemPtr ephem(new Ephem());\n        uint32_t prn = static_cast<uint32_t>(std::stoi(ephem_lines[0].substr(1, 2)));\n        ephem->sat = sat_no(sat_sys, prn);\n        double epoch[6];\n        epoch[0] = static_cast<double>(std::stoi(ephem_lines[0].substr(4, 4)));\n        epoch[1] = static_cast<double>(std::stoi(ephem_lines[0].substr(9, 2)));\n        epoch[2] = static_cast<double>(std::stoi(ephem_lines[0].substr(12, 2)));\n        epoch[3] = static_cast<double>(std::stoi(ephem_lines[0].substr(15, 2)));\n        epoch[4] = static_cast<double>(std::stoi(ephem_lines[0].substr(18, 2)));\n        epoch[5] = static_cast<double>(std::stoi(ephem_lines[0].substr(21, 2)));\n        ephem->toc = epoch2time(epoch);\n        if (sat_sys == SYS_BDS)     ephem->toc.time += 14;     // BDS-GPS time correction\n        ephem->af0 = str2double(ephem_lines[0].substr(23, 19));\n        ephem->af1 = str2double(ephem_lines[0].substr(42, 19));\n        ephem->af2 = str2double(ephem_lines[0].substr(61, 19));\n\n        // the second line\n        if (sat_sys == SYS_GPS)\n            ephem->iode  = str2double(ephem_lines[1].substr(4, 19));\n        ephem->crs       = str2double(ephem_lines[1].substr(23, 19));\n        ephem->delta_n   = str2double(ephem_lines[1].substr(42, 19));\n        ephem->M0        = str2double(ephem_lines[1].substr(61, 19));\n\n        // the third line\n        ephem->cuc = str2double(ephem_lines[2].substr(4, 19));\n        ephem->e = str2double(ephem_lines[2].substr(23, 19));\n        ephem->cus = str2double(ephem_lines[2].substr(42, 19));\n        double sqrt_A = str2double(ephem_lines[2].substr(61, 19));\n        ephem->A = sqrt_A * sqrt_A;\n\n        // the forth line\n        ephem->toe_tow = str2double(ephem_lines[3].substr(4, 19));\n        ephem->cic = str2double(ephem_lines[3].substr(23, 19));\n        ephem->OMG0 = str2double(ephem_lines[3].substr(42, 19));\n        ephem->cis = str2double(ephem_lines[3].substr(61, 19));\n\n        // the fifth line\n        ephem->i0 = str2double(ephem_lines[4].substr(4, 19));\n        ephem->crc = str2double(ephem_lines[4].substr(23, 19));\n        ephem->omg = str2double(ephem_lines[4].substr(42, 19));\n        ephem->OMG_dot = str2double(ephem_lines[4].substr(61, 19));\n\n        // the sixth line\n        ephem->i_dot = str2double(ephem_lines[5].substr(4, 19));\n        if  (sat_sys == SYS_GAL)\n        {\n            uint32_t ephe_source = static_cast<uint32_t>(str2double(ephem_lines[5].substr(23, 19)));\n            if (!(ephe_source & 0x01))  \n            {\n                // LOG(ERROR) << \"not contain I/NAV E1-b info, skip this ephemeris\";\n                return ephem;   // only parse I/NAV E1-b ephemeris\n            }\n        }\n        ephem->week = static_cast<uint32_t>(str2double(ephem_lines[5].substr(42, 19)));\n        if (sat_sys == SYS_GPS || sat_sys == SYS_GAL)     ephem->toe = gpst2time(ephem->week, ephem->toe_tow);\n        else if (sat_sys == SYS_BDS)                      ephem->toe = bdt2time(ephem->week, ephem->toe_tow+14);\n        // if (sat_sys == SYS_GAL)     ephem->toe = gst2time(ephem->week, ephem->toe_tow);\n\n        // the seventh line\n        ephem->ura = str2double(ephem_lines[6].substr(4, 19));\n        ephem->health = static_cast<uint32_t>(str2double(ephem_lines[6].substr(23, 19)));\n        ephem->tgd[0] = str2double(ephem_lines[6].substr(42, 19));\n        if (sat_sys == SYS_BDS || sat_sys == SYS_GAL)\n            ephem->tgd[1] = str2double(ephem_lines[6].substr(61, 19));\n        if (sat_sys == SYS_GPS)     ephem->iodc = str2double(ephem_lines[6].substr(61, 19));\n\n        // the eighth line\n        double ttr_tow = str2double(ephem_lines[7].substr(4, 19));\n        // GAL week = GST week + 1024 + rollover, already align with GPS week!!!\n        if      (sat_sys == SYS_GPS || sat_sys == SYS_GAL)   ephem->ttr = gpst2time(ephem->week, ttr_tow);\n        else if (sat_sys == SYS_BDS)   ephem->ttr = bdt2time(ephem->week, ttr_tow);\n\n        // convert time system to parameter GPST\n        if (sat_sys == SYS_BDS)\n        {\n            uint32_t week = 0;\n            ephem->toe_tow = time2gpst(ephem->toe, &week);\n            ephem->week = week;\n        }\n\n        return ephem;\n    }\n\n    void rinex2ephems(const std::string &rinex_filepath, std::map<uint32_t, std::vector<EphemBasePtr>> &sat2ephem)\n    {\n        uint32_t gpst_leap_seconds = static_cast<uint32_t>(-1);\n        std::ifstream ephem_file(rinex_filepath);\n        std::string line;\n        while(std::getline(ephem_file, line))\n        {\n            if (line.find(\"RINEX VERSION / TYPE\") != std::string::npos && line.find(\"3.04\") == std::string::npos)\n            {\n                LOG(ERROR) << \"Only RINEX 3.04 is supported for observation file\";\n                return;\n            }\n            else if (line.find(\"LEAP SECONDS\") != std::string::npos && line.find(\"BDS\") == std::string::npos)\n                gpst_leap_seconds = static_cast<uint32_t>(std::stoi(line.substr(4, 6)));\n            else if (line.find(\"END OF HEADER\") != std::string::npos)\n                break;\n        }\n        LOG_IF(FATAL, gpst_leap_seconds == static_cast<uint32_t>(-1)) << \"No leap second record found\";\n\n        while(std::getline(ephem_file, line))\n        {\n            if (line.at(0) == 'G' || line.at(0) == 'C' || line.at(0) == 'E')\n            {\n                std::vector<std::string> ephem_lines;\n                ephem_lines.push_back(line);\n                for (size_t i = 0; i < 7; ++i)\n                {\n                    std::getline(ephem_file, line);\n                    ephem_lines.push_back(line);\n                }\n                EphemPtr ephem = rinex_line2ephem(ephem_lines);\n                if (!ephem || ephem->ttr.time == 0)  continue;\n                if (sat2ephem.count(ephem->sat) == 0)\n                    sat2ephem.emplace(ephem->sat, std::vector<EphemBasePtr>());\n                sat2ephem.at(ephem->sat).push_back(ephem);\n            }\n            else if (line.at(0) == 'R')\n            {\n                std::vector<std::string> ephem_lines;\n                ephem_lines.push_back(line);\n                for (size_t i = 0; i < 3; ++i)\n                {\n                    std::getline(ephem_file, line);\n                    ephem_lines.push_back(line);\n                }\n                GloEphemPtr glo_ephem = rinex_line2glo_ephem(ephem_lines, gpst_leap_seconds);\n                if (sat2ephem.count(glo_ephem->sat) == 0)\n                    sat2ephem.emplace(glo_ephem->sat, std::vector<EphemBasePtr>());\n                sat2ephem.at(glo_ephem->sat).push_back(glo_ephem);\n            }\n        }\n    }\n\n    static ObsPtr rinex_line2obs(const std::string rinex_str, \n        const std::map<uint8_t, std::vector<std::string>> &sys2type)\n    {\n        ObsPtr obs;\n        uint8_t sys_char = rinex_str.at(0);\n        if (char2sys.count(sys_char) == 0)   return obs;\n        obs.reset(new Obs());\n        uint32_t sys = char2sys.at(sys_char);\n        uint32_t prn = static_cast<uint32_t>(std::stoi(rinex_str.substr(1, 2)));\n        obs->sat = sat_no(sys, prn);\n        std::map<double, uint32_t> freq2idx;\n        std::vector<std::string> date_types = sys2type.at(sys_char);\n        uint32_t line_offset = 3;\n        for (auto type : date_types)\n        {\n            std::string field_str = rinex_str.substr(line_offset, 14);\n            line_offset += 14 + 2;\n            if (field_str.find_first_not_of(' ') == std::string::npos)  continue;\n            const double field_value = stod(field_str);\n            std::stringstream ss;\n            ss << sys_char << type.at(1);\n            const double freq = type2freq.at(ss.str());\n            uint32_t freq_idx = static_cast<uint32_t>(-1);\n            if (freq2idx.count(freq) == 0)\n            {\n                obs->freqs.push_back(freq);\n                freq_idx = obs->freqs.size()-1;\n                freq2idx.emplace(freq, freq_idx);\n            }\n            else\n            {\n                freq_idx = freq2idx.at(freq);\n            }\n            \n            if (type.at(0) == 'L')\n                obs->cp[freq_idx] = field_value;\n            else if (type.at(0) == 'C')\n                obs->psr[freq_idx] = field_value;\n            else if (type.at(0) == 'D')\n                obs->dopp[freq_idx] = field_value;\n            else if (type.at(0) == 'S')\n                obs->CN0[freq_idx] = field_value;\n            else\n                LOG(FATAL) << \"Unrecognized measurement type \" << type.at(0);\n        }\n        // fill in other fields\n        uint32_t num_freqs = obs->freqs.size();\n        LOG_IF(FATAL, num_freqs < obs->CN0.size()) << \"Suspicious observation field.\\n\";\n        std::fill_n(std::back_inserter(obs->CN0), num_freqs-obs->CN0.size(), 0);\n        LOG_IF(FATAL, num_freqs < obs->LLI.size()) << \"Suspicious observation field.\\n\";\n        std::fill_n(std::back_inserter(obs->LLI), num_freqs-obs->LLI.size(), 0);\n        LOG_IF(FATAL, num_freqs < obs->code.size()) << \"Suspicious observation field.\\n\";\n        std::fill_n(std::back_inserter(obs->code), num_freqs-obs->code.size(), 0);\n        LOG_IF(FATAL, num_freqs < obs->psr.size()) << \"Suspicious observation field.\\n\";\n        std::fill_n(std::back_inserter(obs->psr), num_freqs-obs->psr.size(), 0);\n        LOG_IF(FATAL, num_freqs < obs->psr_std.size()) << \"Suspicious observation field.\\n\";\n        std::fill_n(std::back_inserter(obs->psr_std), num_freqs-obs->psr_std.size(), 0);\n        LOG_IF(FATAL, num_freqs < obs->cp.size()) << \"Suspicious observation field.\\n\";\n        std::fill_n(std::back_inserter(obs->cp), num_freqs-obs->cp.size(), 0);\n        LOG_IF(FATAL, num_freqs < obs->cp_std.size()) << \"Suspicious observation field.\\n\";\n        std::fill_n(std::back_inserter(obs->cp_std), num_freqs-obs->cp_std.size(), 0);\n        LOG_IF(FATAL, num_freqs < obs->dopp.size()) << \"Suspicious observation field.\\n\";\n        std::fill_n(std::back_inserter(obs->dopp), num_freqs-obs->dopp.size(), 0);\n        LOG_IF(FATAL, num_freqs < obs->dopp_std.size()) << \"Suspicious observation field.\\n\";\n        std::fill_n(std::back_inserter(obs->dopp_std), num_freqs-obs->dopp_std.size(), 0);\n        LOG_IF(FATAL, num_freqs < obs->status.size()) << \"Suspicious observation field.\\n\";\n        std::fill_n(std::back_inserter(obs->status), num_freqs-obs->status.size(), 0x0F);\n\n        return obs;\n    }\n\n    // TODO: GLONASS slot number\n    void rinex2obs(const std::string &rinex_filepath, std::vector<std::vector<ObsPtr>> &rinex_meas)\n    {\n        std::ifstream obs_file(rinex_filepath);\n        std::string rinex_line;\n\n        // parse header\n        std::map<uint8_t, std::vector<std::string>> sys2type;\n        uint8_t sys_char = 0;\n        while (std::getline(obs_file, rinex_line))\n        {\n            if (rinex_line.find(\"RINEX VERSION / TYPE\") != std::string::npos && rinex_line.find(\"3.04\") == std::string::npos)\n            {\n                LOG(ERROR) << \"Only RINEX 3.04 is supported for observation file\";\n                return;\n            }\n            else if (rinex_line.find(\"SYS / # / OBS TYPES\") != std::string::npos)\n            {\n                if (rinex_line.at(0) != ' ')\n                {\n                    sys_char = rinex_line.at(0);\n                    sys2type.emplace(sys_char, std::vector<std::string>());\n                }\n                for (size_t i = 0; i < 13; ++i)\n                    if (rinex_line.substr(7+4*i, 3) != \"   \")\n                        sys2type.at(sys_char).emplace_back(rinex_line.substr(7+4*i, 3));\n            }\n            else if (rinex_line.find(\"END OF HEADER\") != std::string::npos)  break;\n        }\n\n        while (std::getline(obs_file, rinex_line))\n        {\n            LOG_IF(FATAL, rinex_line.at(0) != '>') << \"Invalid Observation record \" << rinex_line;\n            LOG_IF(FATAL, rinex_line.at(31) != '0') << \"Invalid Epoch data \" << rinex_line.at(31)-48;\n            std::vector<double> epoch_time;\n            epoch_time.emplace_back(std::stod(rinex_line.substr(2, 4)));\n            epoch_time.emplace_back(std::stod(rinex_line.substr(7, 2)));\n            epoch_time.emplace_back(std::stod(rinex_line.substr(10, 2)));\n            epoch_time.emplace_back(std::stod(rinex_line.substr(13, 2)));\n            epoch_time.emplace_back(std::stod(rinex_line.substr(16, 2)));\n            epoch_time.emplace_back(std::stod(rinex_line.substr(18, 11)));\n            gtime_t obs_time = epoch2time(&(epoch_time[0]));\n            const int num_obs = std::stoi(rinex_line.substr(32, 3));\n            std::vector<ObsPtr> meas;\n            for (int i = 0; i < num_obs; ++i)\n            {\n                LOG_IF(FATAL, !std::getline(obs_file, rinex_line)) << \"Incomplete RINEX file\";\n                ObsPtr obs = rinex_line2obs(rinex_line, sys2type);\n                if (!obs || obs->freqs.empty())  continue;\n                obs->time = obs_time;\n                meas.emplace_back(obs);\n            }\n            rinex_meas.emplace_back(meas);\n        }\n    }\n\n    void rinex2iono_params(const std::string &rinex_filepath, std::vector<double> &iono_params)\n    {\n        iono_params.resize(8);\n        std::ifstream file(rinex_filepath);\n        std::string line;\n\n        // check first line, mainly RINEX version\n        if (!(std::getline(file, line) && line.find(\"RINEX VERSION\")  != std::string::npos \n                && line.find(\"3.04\") != std::string::npos))\n        {\n            LOG(ERROR) << \"Only RINEX 3.04 is supported\";\n            return;\n        }\n\n        bool find_alpha = false, find_beta = false;\n        while(std::getline(file, line))\n        {\n            if (line.find(\"IONOSPHERIC CORR\") != std::string::npos && line.find(\"GPSA\") != std::string::npos)\n            {\n\n                // parse ion alpha value\n                for (size_t i = 0; i < 4; ++i)\n                {\n                    std::string value_str = line.substr(i*12+5, 12);\n                    iono_params[i] = str2double(value_str);\n                }\n                find_alpha = true;\n            }\n            else if (line.find(\"IONOSPHERIC CORR\") != std::string::npos && line.find(\"GPSB\") != std::string::npos)\n            {\n                // parse ion beta value\n                for (size_t i = 0; i < 4; ++i)\n                {\n                    std::string value_str = line.substr(i*12+5, 12);\n                    iono_params[i+4] = str2double(value_str);\n                }\n                find_beta = true;\n            }\n\n            if(find_alpha && find_beta)\n                break;\n        }\n        file.close();\n    }\n\n    int obs_index(const uint32_t sys, const double freq)\n    {\n        if (sys == SYS_GPS)\n        {\n            if (freq == FREQ1)          return 0;\n            else if (freq == FREQ2)     return 1;\n        }\n        else if (sys == SYS_GLO)\n        {\n            if (freq <= (FREQ1_GLO+6*DFRQ1_GLO))        return 0;\n            else if (freq <= (FREQ2_GLO+6*DFRQ2_GLO))   return 1;\n        }\n        else if (sys == SYS_GAL)\n        {\n            if (freq == FREQ1)          return 0;\n            else if (freq == FREQ7)     return 1;\n        }\n        else if (sys == SYS_BDS)\n        {\n            if (freq == FREQ1_BDS)          return 0;\n            else if (freq == FREQ2_BDS)     return 1;\n        }\n        return -1;\n    }\n\n    double index_freq(const ObsPtr &obs, const int band_idx, int &freq_idx)\n    {\n        freq_idx = -1;      // set frequency index of `obs->freqs` invalid\n        const uint32_t sys = satsys(obs->sat, NULL);\n\n        // calculate target band frequency\n        double freq_min = 0;\n        double freq_max = 0;\n        if (sys == SYS_GPS && band_idx == 0)\n        {\n            freq_min = freq_max = FREQ1;\n        }\n        else if (sys == SYS_GPS && band_idx == 1)\n        {\n            freq_min = freq_max = FREQ2;\n        }\n        else if (sys == SYS_GLO && band_idx == 0)\n        {\n            freq_min = FREQ1_GLO-7*DFRQ1_GLO;\n            freq_max = FREQ1_GLO+6*DFRQ1_GLO;\n        }\n        else if (sys == SYS_GLO && band_idx == 1)\n        {\n            freq_min = FREQ2_GLO-7*DFRQ2_GLO;\n            freq_max = FREQ2_GLO+6*DFRQ2_GLO;\n        }\n        else if (sys == SYS_GAL && band_idx == 0)\n        {\n            freq_min = freq_max = FREQ1;\n        }\n        else if (sys == SYS_GAL && band_idx == 1)\n        {\n            freq_min = freq_max = FREQ7;\n        }\n        else if (sys == SYS_BDS && band_idx == 0)\n        {\n            freq_min = freq_max = FREQ1_BDS;\n        }\n        else if (sys == SYS_BDS && band_idx == 1)\n        {\n            freq_min = freq_max = FREQ2_BDS;\n        }\n\n        // check if target frequency exists or not\n        for (uint32_t i = 0; i < obs->freqs.size(); ++i)\n        {\n            if (obs->freqs[i] >= freq_min && obs->freqs[i] <= freq_max)\n            {\n                freq_idx = static_cast<int>(i);\n                return obs->freqs[i];\n            }\n        }\n\n        return -1.0;\n    }\n\n    std::map<uint32_t, int> get_glo_freqo(const std::vector<std::vector<ObsPtr>> &gnss_meas)\n    {\n        std::map<uint32_t, int> sat2freqo;\n        for (auto &meas : gnss_meas)\n        {\n            for (auto &obs : meas)\n            {\n                const uint32_t sys = satsys(obs->sat, NULL);\n                if (sys != SYS_GLO)     continue;\n\n                for (double freq : obs->freqs)\n                {\n                    if (freq > FREQ1_GLO-8*DFRQ1_GLO && freq < FREQ1_GLO+7*DFRQ1_GLO)\n                    {\n                        const int freqo = round((freq-FREQ1_GLO)/DFRQ1_GLO);\n                        sat2freqo.emplace(obs->sat, freqo);\n                    }\n                    else if (freq > FREQ2_GLO-8*DFRQ2_GLO && freq < FREQ2_GLO+7*DFRQ2_GLO)\n                    {\n                        const int freqo = round((freq-FREQ2_GLO)/DFRQ2_GLO);\n                        sat2freqo.emplace(obs->sat, freqo);\n                    }\n                }\n            }\n        }\n        return sat2freqo;\n    }\n\n    void obs2rinex(const std::string &rinex_filepath, \n        const std::vector<std::vector<ObsPtr>> &gnss_meas)\n    {\n        FILE *fp = fopen(rinex_filepath.c_str(), \"w\");\n        fprintf(fp, \"%9.2f%-11s%-20s%-20s%-20s\\n\", 3.04, \"\", \"OBSERVATION DATA\", \n            \"M: Mixed\", \"RINEX VERSION / TYPE\");\n        std::time_t time_ptr;\n        time_ptr = time(NULL);\n        tm *tm_utc = gmtime(&time_ptr);\n        char date_str[256];\n        sprintf(date_str, \"%4d%02d%02d %02d%02d%02d UTC\", tm_utc->tm_year+1900, tm_utc->tm_mon+1, \n            tm_utc->tm_mday, tm_utc->tm_hour, tm_utc->tm_min, tm_utc->tm_sec);\n        fprintf(fp, \"%-20.20s%-20.20s%-20.20s%-20s\\n\",\"gnss_comm obs2rinex\", \"\", date_str,\n            \"PGM / RUN BY / DATE\");\n        fprintf(fp, \"%-60.60s%-20s\\n\", \"\", \"MARKER NAME\");\n        fprintf(fp, \"%-20.20s%-40.40s%-20s\\n\", \"\", \"\", \"MARKER NUMBER\");\n        fprintf(fp, \"%-20.20s%-40.40s%-20s\\n\", \"\", \"\", \"MARKER TYPE\");\n        fprintf(fp, \"%-20.20s%-40.40s%-20s\\n\", \"\", \"\", \"OBSERVER / AGENCY\");\n        fprintf(fp, \"%-20.20s%-20.20s%-20.20s%-20s\\n\", \"\", \"\", \"\", \"REC # / TYPE / VERS\");\n        fprintf(fp, \"%-20.20s%-20.20s%-20.20s%-20s\\n\", \"\", \"\", \"\", \"ANT # / TYPE\");\n        // ignore approximate position\n        fprintf(fp, \"%-14.14s%-14.14s%-14.14s%-18s%-20s\\n\", \"\", \"\", \"\", \"\", \"APPROX POSITION XYZ\");\n        fprintf(fp, \"%-14.14s%-14.14s%-14.14s%-18s%-20s\\n\", \"\", \"\", \"\", \"\", \"ANTENNA: DELTA H/E/N\");\n\n        // observation type, hard code here\n        // only support dual-frequency record without the knowledge of signal type\n        fprintf(fp, \"%c  %3d %3s %3s %3s %3s %3s %3s %3s %3s%20s  %-20s\\n\", 'G', 8, \"C1C\", \"L1C\", \n            \"D1C\", \"S1C\", \"C2S\", \"L2S\", \"D2S\", \"S2S\", \"\", \"SYS / # / OBS TYPES \");\n        fprintf(fp, \"%c  %3d %3s %3s %3s %3s %3s %3s %3s %3s%20s  %-20s\\n\", 'R', 8, \"C1C\", \"L1C\", \n            \"D1C\", \"S1C\", \"C2C\", \"L2C\", \"D2C\", \"S2C\", \"\", \"SYS / # / OBS TYPES \");\n        fprintf(fp, \"%c  %3d %3s %3s %3s %3s %3s %3s %3s %3s%20s  %-20s\\n\", 'E', 8, \"C1C\", \"L1C\", \n            \"D1C\", \"S1C\", \"C7Q\", \"L7Q\", \"D7Q\", \"S7Q\", \"\", \"SYS / # / OBS TYPES \");\n        fprintf(fp, \"%c  %3d %3s %3s %3s %3s %3s %3s %3s %3s%20s  %-20s\\n\", 'C', 8, \"C2I\", \"L2I\", \n            \"D2I\", \"S2I\", \"C7I\", \"L7I\", \"D7I\", \"S7I\", \"\", \"SYS / # / OBS TYPES \");\n        \n        if (!gnss_meas.empty())\n        {\n            int front_idx = -1;\n            while(gnss_meas[++front_idx].empty());\n            const gtime_t start_time = gnss_meas[front_idx].front()->time;\n            double ep[6];\n            time2epoch(start_time, ep);\n            fprintf(fp, \"  %04.0f%6.0f%6.0f%6.0f%6.0f%13.7f     %-12s%-20s\\n\", ep[0], \n                ep[1], ep[2], ep[3], ep[4], ep[5], \"GPS\", \"TIME OF FIRST OBS\");\n            int back_idx = static_cast<int>(gnss_meas.size());\n            while(gnss_meas[--back_idx].empty());\n            const gtime_t end_time = gnss_meas[back_idx].front()->time;\n            time2epoch(end_time, ep);\n            fprintf(fp, \"  %04.0f%6.0f%6.0f%6.0f%6.0f%13.7f     %-12s%-20s\\n\", ep[0], \n                ep[1], ep[2], ep[3], ep[4], ep[5], \"GPS\", \"TIME OF LAST OBS\");\n        }\n        fprintf(fp,\"%c %-58s%-20s\\n\",'G',\"\",\"SYS / PHASE SHIFT\");\n        fprintf(fp,\"%c %-58s%-20s\\n\",'R',\"\",\"SYS / PHASE SHIFT\");\n        fprintf(fp,\"%c %-58s%-20s\\n\",'E',\"\",\"SYS / PHASE SHIFT\");\n        fprintf(fp,\"%c %-58s%-20s\\n\",'C',\"\",\"SYS / PHASE SHIFT\");\n\n        // add GLONASS SLOT / FRQ #\n        std::map<uint32_t, int> glo_sat2freqo = get_glo_freqo(gnss_meas);\n        auto glo_sat2freqo_it = glo_sat2freqo.begin();\n        const int num_glo_sats = static_cast<int>(glo_sat2freqo.size());\n        for (int i = 0; i < (num_glo_sats<=0?1:(num_glo_sats-1)/8+1); ++i)\n        {\n            if (i == 0)\n                fprintf(fp, \"%3d\", num_glo_sats);\n            else\n                fprintf(fp, \"%3s\", \"\");\n            for (int j = 0; j < 8; ++j)\n            {\n                if (i*8+j < num_glo_sats)\n                {\n                    fprintf(fp, \" %3s %2d\", sat2str(glo_sat2freqo_it->first).c_str(), glo_sat2freqo_it->second);\n                    ++ glo_sat2freqo_it;\n                }\n                else\n                {\n                    fprintf(fp, \" %6s\", \"\");\n                }\n            }\n            fprintf(fp, \" %-20s\\n\", \"GLONASS SLOT / FRQ #\");\n        }\n        fprintf(fp,\" C1C    0.000 C1P    0.000 C2C    0.000 C2P    0.000        GLONASS COD/PHS/BIS \\n\");\n        fprintf(fp,\"%-60.60s%-20s\\n\",\"\",\"END OF HEADER\");\n\n        // write data\n        for (auto &meas : gnss_meas)\n        {\n            if (meas.empty())         continue;\n            double ep[6];\n            time2epoch(meas[0]->time, ep);\n            fprintf(fp, \"> %04.0f %2.0f %2.0f %2.0f %2.0f%11.7f  %d%3d%21s\\n\", \n                ep[0], ep[1], ep[2], ep[3], ep[4], ep[5], 0, static_cast<int>(meas.size()), \"\");\n            \n            std::vector<ObsPtr> sorted_meas(meas);\n            std::sort(sorted_meas.begin(), sorted_meas.end(), [](ObsPtr o1, ObsPtr o2) {\n                return o1->sat < o2->sat;\n            });\n            \n            for (auto &obs : sorted_meas)\n            {\n                std::string sat_str = sat2str(obs->sat);\n                fprintf(fp, \"%3s\", sat_str.c_str());\n                for (int k = 0; k < 2; ++k)\n                {\n                    int freq_idx = -1;\n                    const double freq = index_freq(obs, k, freq_idx);\n                    if (freq < 0)\n                    {\n                        fprintf(fp, \"%16s%16s%16s%16s\", \"\", \"\", \"\", \"\");\n                    }\n                    else \n                    {\n                        fprintf(fp, \"%14.3f%2s\", obs->psr[freq_idx], \"\");\n                        fprintf(fp, \"%14.3f%2s\", obs->cp[freq_idx], \"\");\n                        fprintf(fp, \"%14.3f%2s\", obs->dopp[freq_idx], \"\");\n                        fprintf(fp, \"%14.3f%2s\", obs->CN0[freq_idx], \"\");\n                    }\n                }\n                fprintf(fp, \"\\n\");\n            }\n        }\n\n        fclose(fp);\n    }\n}   // namespace gnss_comm\n"
  },
  {
    "path": "ingvio_estimator/CMakeLists.txt",
    "content": "cmake_minimum_required(VERSION 3.16)\nproject(ingvio_estimator)\n\nset(CMAKE_CXX_STANDARD 14)\nset(CMAKE_CXX_STANDARD_REQUIRED ON)\nset(CMAKE_CXX_EXTENSIONS OFF)\n\nset(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} \"${CMAKE_CURRENT_LIST_DIR}/cmake\")\n\nif (NOT CMAKE_BUILD_TYPE)\n    set(CMAKE_BUILD_TYPE \"Release\")\nendif ()\n\n# set(CMAKE_CXX_FLAGS \"${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops\")\n\nfind_package(Eigen3 QUIET)\nif (NOT Eigen3_FOUND)\n    set(EIGEN3_INCLUDE_DIR \"/usr/include/eigen3\")\nendif ()\n\nfind_package(OpenCV 3 QUIET)\nif (NOT OpenCV_FOUND)\n    find_package(OpenCV 4 REQUIRED)\nendif ()\n\nfind_package(SuiteSparse REQUIRED)\n\nfind_package(Boost REQUIRED COMPONENTS system filesystem thread date_time)\n\nfind_package(catkin QUIET COMPONENTS roscpp)\nfind_package(ament_cmake QUIET)\n\nif (catkin_FOUND)\n    message(STATUS \"ROS *1* version found!\")\n    include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/ROS1.cmake)\nelseif (ament_cmake_FOUND)\n    message(STATUS \"ROS *2* version found! Not supported!\")\n    include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/ROS2.cmake)\nelse ()\n    message(STATUS \"No ROS version found! Cannot build!\")\nendif ()\n\n\n"
  },
  {
    "path": "ingvio_estimator/LICENSE",
    "content": "                    GNU GENERAL PUBLIC LICENSE\n                       Version 3, 29 June 2007\n\n Copyright (C) 2007 Free Software Foundation, Inc. <https://fsf.org/>\n Everyone is permitted to copy and distribute verbatim copies\n of this license document, but changing it is not allowed.\n\n                            Preamble\n\n  The GNU General Public License is a free, copyleft license for\nsoftware and other kinds of works.\n\n  The licenses for most software and other practical works are designed\nto take away your freedom to share and change the works.  By contrast,\nthe GNU General Public License is intended to guarantee your freedom to\nshare and change all versions of a program--to make sure it remains free\nsoftware for all its users.  We, the Free Software Foundation, use the\nGNU General Public License for most of our software; it applies also to\nany other work released this way by its authors.  You can apply it to\nyour programs, too.\n\n  When we speak of free software, we are referring to freedom, not\nprice.  Our General Public Licenses are designed to make sure that you\nhave the freedom to distribute copies of free software (and charge for\nthem if you wish), that you receive source code or can get it if you\nwant it, that you can change the software or use pieces of it in new\nfree programs, and that you know you can do these things.\n\n  To protect your rights, we need to prevent others from denying you\nthese rights or asking you to surrender the rights.  Therefore, you have\ncertain responsibilities if you distribute copies of the software, or if\nyou modify it: responsibilities to respect the freedom of others.\n\n  For example, if you distribute copies of such a program, whether\ngratis or for a fee, you must pass on to the recipients the same\nfreedoms that you received.  You must make sure that they, too, receive\nor can get the source code.  And you must show them these terms so they\nknow their rights.\n\n  Developers that use the GNU GPL protect your rights with two steps:\n(1) assert copyright on the software, and (2) offer you this License\ngiving you legal permission to copy, distribute and/or modify it.\n\n  For the developers' and authors' protection, the GPL clearly explains\nthat there is no warranty for this free software.  For both users' and\nauthors' sake, the GPL requires that modified versions be marked as\nchanged, so that their problems will not be attributed erroneously to\nauthors of previous versions.\n\n  Some devices are designed to deny users access to install or run\nmodified versions of the software inside them, although the manufacturer\ncan do so.  This is fundamentally incompatible with the aim of\nprotecting users' freedom to change the software.  The systematic\npattern of such abuse occurs in the area of products for individuals to\nuse, which is precisely where it is most unacceptable.  Therefore, we\nhave designed this version of the GPL to prohibit the practice for those\nproducts.  If such problems arise substantially in other domains, we\nstand ready to extend this provision to those domains in future versions\nof the GPL, as needed to protect the freedom of users.\n\n  Finally, every program is threatened constantly by software patents.\nStates should not allow patents to restrict development and use of\nsoftware on general-purpose computers, but in those that do, we wish to\navoid the special danger that patents applied to a free program could\nmake it effectively proprietary.  To prevent this, the GPL assures that\npatents cannot be used to render the program non-free.\n\n  The precise terms and conditions for copying, distribution and\nmodification follow.\n\n                       TERMS AND CONDITIONS\n\n  0. Definitions.\n\n  \"This License\" refers to version 3 of the GNU General Public License.\n\n  \"Copyright\" also means copyright-like laws that apply to other kinds of\nworks, such as semiconductor masks.\n\n  \"The Program\" refers to any copyrightable work licensed under this\nLicense.  Each licensee is addressed as \"you\".  \"Licensees\" and\n\"recipients\" may be individuals or organizations.\n\n  To \"modify\" a work means to copy from or adapt all or part of the work\nin a fashion requiring copyright permission, other than the making of an\nexact copy.  The resulting work is called a \"modified version\" of the\nearlier work or a work \"based on\" the earlier work.\n\n  A \"covered work\" means either the unmodified Program or a work based\non the Program.\n\n  To \"propagate\" a work means to do anything with it that, without\npermission, would make you directly or secondarily liable for\ninfringement under applicable copyright law, except executing it on a\ncomputer or modifying a private copy.  Propagation includes copying,\ndistribution (with or without modification), making available to the\npublic, and in some countries other activities as well.\n\n  To \"convey\" a work means any kind of propagation that enables other\nparties to make or receive copies.  Mere interaction with a user through\na computer network, with no transfer of a copy, is not conveying.\n\n  An interactive user interface displays \"Appropriate Legal Notices\"\nto the extent that it includes a convenient and prominently visible\nfeature that (1) displays an appropriate copyright notice, and (2)\ntells the user that there is no warranty for the work (except to the\nextent that warranties are provided), that licensees may convey the\nwork under this License, and how to view a copy of this License.  If\nthe interface presents a list of user commands or options, such as a\nmenu, a prominent item in the list meets this criterion.\n\n  1. Source Code.\n\n  The \"source code\" for a work means the preferred form of the work\nfor making modifications to it.  \"Object code\" means any non-source\nform of a work.\n\n  A \"Standard Interface\" means an interface that either is an official\nstandard defined by a recognized standards body, or, in the case of\ninterfaces specified for a particular programming language, one that\nis widely used among developers working in that language.\n\n  The \"System Libraries\" of an executable work include anything, other\nthan the work as a whole, that (a) is included in the normal form of\npackaging a Major Component, but which is not part of that Major\nComponent, and (b) serves only to enable use of the work with that\nMajor Component, or to implement a Standard Interface for which an\nimplementation is available to the public in source code form.  A\n\"Major Component\", in this context, means a major essential component\n(kernel, window system, and so on) of the specific operating system\n(if any) on which the executable work runs, or a compiler used to\nproduce the work, or an object code interpreter used to run it.\n\n  The \"Corresponding Source\" for a work in object code form means all\nthe source code needed to generate, install, and (for an executable\nwork) run the object code and to modify the work, including scripts to\ncontrol those activities.  However, it does not include the work's\nSystem Libraries, or general-purpose tools or generally available free\nprograms which are used unmodified in performing those activities but\nwhich are not part of the work.  For example, Corresponding Source\nincludes interface definition files associated with source files for\nthe work, and the source code for shared libraries and dynamically\nlinked subprograms that the work is specifically designed to require,\nsuch as by intimate data communication or control flow between those\nsubprograms and other parts of the work.\n\n  The Corresponding Source need not include anything that users\ncan regenerate automatically from other parts of the Corresponding\nSource.\n\n  The Corresponding Source for a work in source code form is that\nsame work.\n\n  2. Basic Permissions.\n\n  All rights granted under this License are granted for the term of\ncopyright on the Program, and are irrevocable provided the stated\nconditions are met.  This License explicitly affirms your unlimited\npermission to run the unmodified Program.  The output from running a\ncovered work is covered by this License only if the output, given its\ncontent, constitutes a covered work.  This License acknowledges your\nrights of fair use or other equivalent, as provided by copyright law.\n\n  You may make, run and propagate covered works that you do not\nconvey, without conditions so long as your license otherwise remains\nin force.  You may convey covered works to others for the sole purpose\nof having them make modifications exclusively for you, or provide you\nwith facilities for running those works, provided that you comply with\nthe terms of this License in conveying all material for which you do\nnot control copyright.  Those thus making or running the covered works\nfor you must do so exclusively on your behalf, under your direction\nand control, on terms that prohibit them from making any copies of\nyour copyrighted material outside their relationship with you.\n\n  Conveying under any other circumstances is permitted solely under\nthe conditions stated below.  Sublicensing is not allowed; section 10\nmakes it unnecessary.\n\n  3. Protecting Users' Legal Rights From Anti-Circumvention Law.\n\n  No covered work shall be deemed part of an effective technological\nmeasure under any applicable law fulfilling obligations under article\n11 of the WIPO copyright treaty adopted on 20 December 1996, or\nsimilar laws prohibiting or restricting circumvention of such\nmeasures.\n\n  When you convey a covered work, you waive any legal power to forbid\ncircumvention of technological measures to the extent such circumvention\nis effected by exercising rights under this License with respect to\nthe covered work, and you disclaim any intention to limit operation or\nmodification of the work as a means of enforcing, against the work's\nusers, your or third parties' legal rights to forbid circumvention of\ntechnological measures.\n\n  4. Conveying Verbatim Copies.\n\n  You may convey verbatim copies of the Program's source code as you\nreceive it, in any medium, provided that you conspicuously and\nappropriately publish on each copy an appropriate copyright notice;\nkeep intact all notices stating that this License and any\nnon-permissive terms added in accord with section 7 apply to the code;\nkeep intact all notices of the absence of any warranty; and give all\nrecipients a copy of this License along with the Program.\n\n  You may charge any price or no price for each copy that you convey,\nand you may offer support or warranty protection for a fee.\n\n  5. Conveying Modified Source Versions.\n\n  You may convey a work based on the Program, or the modifications to\nproduce it from the Program, in the form of source code under the\nterms of section 4, provided that you also meet all of these conditions:\n\n    a) The work must carry prominent notices stating that you modified\n    it, and giving a relevant date.\n\n    b) The work must carry prominent notices stating that it is\n    released under this License and any conditions added under section\n    7.  This requirement modifies the requirement in section 4 to\n    \"keep intact all notices\".\n\n    c) You must license the entire work, as a whole, under this\n    License to anyone who comes into possession of a copy.  This\n    License will therefore apply, along with any applicable section 7\n    additional terms, to the whole of the work, and all its parts,\n    regardless of how they are packaged.  This License gives no\n    permission to license the work in any other way, but it does not\n    invalidate such permission if you have separately received it.\n\n    d) If the work has interactive user interfaces, each must display\n    Appropriate Legal Notices; however, if the Program has interactive\n    interfaces that do not display Appropriate Legal Notices, your\n    work need not make them do so.\n\n  A compilation of a covered work with other separate and independent\nworks, which are not by their nature extensions of the covered work,\nand which are not combined with it such as to form a larger program,\nin or on a volume of a storage or distribution medium, is called an\n\"aggregate\" if the compilation and its resulting copyright are not\nused to limit the access or legal rights of the compilation's users\nbeyond what the individual works permit.  Inclusion of a covered work\nin an aggregate does not cause this License to apply to the other\nparts of the aggregate.\n\n  6. Conveying Non-Source Forms.\n\n  You may convey a covered work in object code form under the terms\nof sections 4 and 5, provided that you also convey the\nmachine-readable Corresponding Source under the terms of this License,\nin one of these ways:\n\n    a) Convey the object code in, or embodied in, a physical product\n    (including a physical distribution medium), accompanied by the\n    Corresponding Source fixed on a durable physical medium\n    customarily used for software interchange.\n\n    b) Convey the object code in, or embodied in, a physical product\n    (including a physical distribution medium), accompanied by a\n    written offer, valid for at least three years and valid for as\n    long as you offer spare parts or customer support for that product\n    model, to give anyone who possesses the object code either (1) a\n    copy of the Corresponding Source for all the software in the\n    product that is covered by this License, on a durable physical\n    medium customarily used for software interchange, for a price no\n    more than your reasonable cost of physically performing this\n    conveying of source, or (2) access to copy the\n    Corresponding Source from a network server at no charge.\n\n    c) Convey individual copies of the object code with a copy of the\n    written offer to provide the Corresponding Source.  This\n    alternative is allowed only occasionally and noncommercially, and\n    only if you received the object code with such an offer, in accord\n    with subsection 6b.\n\n    d) Convey the object code by offering access from a designated\n    place (gratis or for a charge), and offer equivalent access to the\n    Corresponding Source in the same way through the same place at no\n    further charge.  You need not require recipients to copy the\n    Corresponding Source along with the object code.  If the place to\n    copy the object code is a network server, the Corresponding Source\n    may be on a different server (operated by you or a third party)\n    that supports equivalent copying facilities, provided you maintain\n    clear directions next to the object code saying where to find the\n    Corresponding Source.  Regardless of what server hosts the\n    Corresponding Source, you remain obligated to ensure that it is\n    available for as long as needed to satisfy these requirements.\n\n    e) Convey the object code using peer-to-peer transmission, provided\n    you inform other peers where the object code and Corresponding\n    Source of the work are being offered to the general public at no\n    charge under subsection 6d.\n\n  A separable portion of the object code, whose source code is excluded\nfrom the Corresponding Source as a System Library, need not be\nincluded in conveying the object code work.\n\n  A \"User Product\" is either (1) a \"consumer product\", which means any\ntangible personal property which is normally used for personal, family,\nor household purposes, or (2) anything designed or sold for incorporation\ninto a dwelling.  In determining whether a product is a consumer product,\ndoubtful cases shall be resolved in favor of coverage.  For a particular\nproduct received by a particular user, \"normally used\" refers to a\ntypical or common use of that class of product, regardless of the status\nof the particular user or of the way in which the particular user\nactually uses, or expects or is expected to use, the product.  A product\nis a consumer product regardless of whether the product has substantial\ncommercial, industrial or non-consumer uses, unless such uses represent\nthe only significant mode of use of the product.\n\n  \"Installation Information\" for a User Product means any methods,\nprocedures, authorization keys, or other information required to install\nand execute modified versions of a covered work in that User Product from\na modified version of its Corresponding Source.  The information must\nsuffice to ensure that the continued functioning of the modified object\ncode is in no case prevented or interfered with solely because\nmodification has been made.\n\n  If you convey an object code work under this section in, or with, or\nspecifically for use in, a User Product, and the conveying occurs as\npart of a transaction in which the right of possession and use of the\nUser Product is transferred to the recipient in perpetuity or for a\nfixed term (regardless of how the transaction is characterized), the\nCorresponding Source conveyed under this section must be accompanied\nby the Installation Information.  But this requirement does not apply\nif neither you nor any third party retains the ability to install\nmodified object code on the User Product (for example, the work has\nbeen installed in ROM).\n\n  The requirement to provide Installation Information does not include a\nrequirement to continue to provide support service, warranty, or updates\nfor a work that has been modified or installed by the recipient, or for\nthe User Product in which it has been modified or installed.  Access to a\nnetwork may be denied when the modification itself materially and\nadversely affects the operation of the network or violates the rules and\nprotocols for communication across the network.\n\n  Corresponding Source conveyed, and Installation Information provided,\nin accord with this section must be in a format that is publicly\ndocumented (and with an implementation available to the public in\nsource code form), and must require no special password or key for\nunpacking, reading or copying.\n\n  7. Additional Terms.\n\n  \"Additional permissions\" are terms that supplement the terms of this\nLicense by making exceptions from one or more of its conditions.\nAdditional permissions that are applicable to the entire Program shall\nbe treated as though they were included in this License, to the extent\nthat they are valid under applicable law.  If additional permissions\napply only to part of the Program, that part may be used separately\nunder those permissions, but the entire Program remains governed by\nthis License without regard to the additional permissions.\n\n  When you convey a copy of a covered work, you may at your option\nremove any additional permissions from that copy, or from any part of\nit.  (Additional permissions may be written to require their own\nremoval in certain cases when you modify the work.)  You may place\nadditional permissions on material, added by you to a covered work,\nfor which you have or can give appropriate copyright permission.\n\n  Notwithstanding any other provision of this License, for material you\nadd to a covered work, you may (if authorized by the copyright holders of\nthat material) supplement the terms of this License with terms:\n\n    a) Disclaiming warranty or limiting liability differently from the\n    terms of sections 15 and 16 of this License; or\n\n    b) Requiring preservation of specified reasonable legal notices or\n    author attributions in that material or in the Appropriate Legal\n    Notices displayed by works containing it; or\n\n    c) Prohibiting misrepresentation of the origin of that material, or\n    requiring that modified versions of such material be marked in\n    reasonable ways as different from the original version; or\n\n    d) Limiting the use for publicity purposes of names of licensors or\n    authors of the material; or\n\n    e) Declining to grant rights under trademark law for use of some\n    trade names, trademarks, or service marks; or\n\n    f) Requiring indemnification of licensors and authors of that\n    material by anyone who conveys the material (or modified versions of\n    it) with contractual assumptions of liability to the recipient, for\n    any liability that these contractual assumptions directly impose on\n    those licensors and authors.\n\n  All other non-permissive additional terms are considered \"further\nrestrictions\" within the meaning of section 10.  If the Program as you\nreceived it, or any part of it, contains a notice stating that it is\ngoverned by this License along with a term that is a further\nrestriction, you may remove that term.  If a license document contains\na further restriction but permits relicensing or conveying under this\nLicense, you may add to a covered work material governed by the terms\nof that license document, provided that the further restriction does\nnot survive such relicensing or conveying.\n\n  If you add terms to a covered work in accord with this section, you\nmust place, in the relevant source files, a statement of the\nadditional terms that apply to those files, or a notice indicating\nwhere to find the applicable terms.\n\n  Additional terms, permissive or non-permissive, may be stated in the\nform of a separately written license, or stated as exceptions;\nthe above requirements apply either way.\n\n  8. Termination.\n\n  You may not propagate or modify a covered work except as expressly\nprovided under this License.  Any attempt otherwise to propagate or\nmodify it is void, and will automatically terminate your rights under\nthis License (including any patent licenses granted under the third\nparagraph of section 11).\n\n  However, if you cease all violation of this License, then your\nlicense from a particular copyright holder is reinstated (a)\nprovisionally, unless and until the copyright holder explicitly and\nfinally terminates your license, and (b) permanently, if the copyright\nholder fails to notify you of the violation by some reasonable means\nprior to 60 days after the cessation.\n\n  Moreover, your license from a particular copyright holder is\nreinstated permanently if the copyright holder notifies you of the\nviolation by some reasonable means, this is the first time you have\nreceived notice of violation of this License (for any work) from that\ncopyright holder, and you cure the violation prior to 30 days after\nyour receipt of the notice.\n\n  Termination of your rights under this section does not terminate the\nlicenses of parties who have received copies or rights from you under\nthis License.  If your rights have been terminated and not permanently\nreinstated, you do not qualify to receive new licenses for the same\nmaterial under section 10.\n\n  9. Acceptance Not Required for Having Copies.\n\n  You are not required to accept this License in order to receive or\nrun a copy of the Program.  Ancillary propagation of a covered work\noccurring solely as a consequence of using peer-to-peer transmission\nto receive a copy likewise does not require acceptance.  However,\nnothing other than this License grants you permission to propagate or\nmodify any covered work.  These actions infringe copyright if you do\nnot accept this License.  Therefore, by modifying or propagating a\ncovered work, you indicate your acceptance of this License to do so.\n\n  10. Automatic Licensing of Downstream Recipients.\n\n  Each time you convey a covered work, the recipient automatically\nreceives a license from the original licensors, to run, modify and\npropagate that work, subject to this License.  You are not responsible\nfor enforcing compliance by third parties with this License.\n\n  An \"entity transaction\" is a transaction transferring control of an\norganization, or substantially all assets of one, or subdividing an\norganization, or merging organizations.  If propagation of a covered\nwork results from an entity transaction, each party to that\ntransaction who receives a copy of the work also receives whatever\nlicenses to the work the party's predecessor in interest had or could\ngive under the previous paragraph, plus a right to possession of the\nCorresponding Source of the work from the predecessor in interest, if\nthe predecessor has it or can get it with reasonable efforts.\n\n  You may not impose any further restrictions on the exercise of the\nrights granted or affirmed under this License.  For example, you may\nnot impose a license fee, royalty, or other charge for exercise of\nrights granted under this License, and you may not initiate litigation\n(including a cross-claim or counterclaim in a lawsuit) alleging that\nany patent claim is infringed by making, using, selling, offering for\nsale, or importing the Program or any portion of it.\n\n  11. Patents.\n\n  A \"contributor\" is a copyright holder who authorizes use under this\nLicense of the Program or a work on which the Program is based.  The\nwork thus licensed is called the contributor's \"contributor version\".\n\n  A contributor's \"essential patent claims\" are all patent claims\nowned or controlled by the contributor, whether already acquired or\nhereafter acquired, that would be infringed by some manner, permitted\nby this License, of making, using, or selling its contributor version,\nbut do not include claims that would be infringed only as a\nconsequence of further modification of the contributor version.  For\npurposes of this definition, \"control\" includes the right to grant\npatent sublicenses in a manner consistent with the requirements of\nthis License.\n\n  Each contributor grants you a non-exclusive, worldwide, royalty-free\npatent license under the contributor's essential patent claims, to\nmake, use, sell, offer for sale, import and otherwise run, modify and\npropagate the contents of its contributor version.\n\n  In the following three paragraphs, a \"patent license\" is any express\nagreement or commitment, however denominated, not to enforce a patent\n(such as an express permission to practice a patent or covenant not to\nsue for patent infringement).  To \"grant\" such a patent license to a\nparty means to make such an agreement or commitment not to enforce a\npatent against the party.\n\n  If you convey a covered work, knowingly relying on a patent license,\nand the Corresponding Source of the work is not available for anyone\nto copy, free of charge and under the terms of this License, through a\npublicly available network server or other readily accessible means,\nthen you must either (1) cause the Corresponding Source to be so\navailable, or (2) arrange to deprive yourself of the benefit of the\npatent license for this particular work, or (3) arrange, in a manner\nconsistent with the requirements of this License, to extend the patent\nlicense to downstream recipients.  \"Knowingly relying\" means you have\nactual knowledge that, but for the patent license, your conveying the\ncovered work in a country, or your recipient's use of the covered work\nin a country, would infringe one or more identifiable patents in that\ncountry that you have reason to believe are valid.\n\n  If, pursuant to or in connection with a single transaction or\narrangement, you convey, or propagate by procuring conveyance of, a\ncovered work, and grant a patent license to some of the parties\nreceiving the covered work authorizing them to use, propagate, modify\nor convey a specific copy of the covered work, then the patent license\nyou grant is automatically extended to all recipients of the covered\nwork and works based on it.\n\n  A patent license is \"discriminatory\" if it does not include within\nthe scope of its coverage, prohibits the exercise of, or is\nconditioned on the non-exercise of one or more of the rights that are\nspecifically granted under this License.  You may not convey a covered\nwork if you are a party to an arrangement with a third party that is\nin the business of distributing software, under which you make payment\nto the third party based on the extent of your activity of conveying\nthe work, and under which the third party grants, to any of the\nparties who would receive the covered work from you, a discriminatory\npatent license (a) in connection with copies of the covered work\nconveyed by you (or copies made from those copies), or (b) primarily\nfor and in connection with specific products or compilations that\ncontain the covered work, unless you entered into that arrangement,\nor that patent license was granted, prior to 28 March 2007.\n\n  Nothing in this License shall be construed as excluding or limiting\nany implied license or other defenses to infringement that may\notherwise be available to you under applicable patent law.\n\n  12. No Surrender of Others' Freedom.\n\n  If conditions are imposed on you (whether by court order, agreement or\notherwise) that contradict the conditions of this License, they do not\nexcuse you from the conditions of this License.  If you cannot convey a\ncovered work so as to satisfy simultaneously your obligations under this\nLicense and any other pertinent obligations, then as a consequence you may\nnot convey it at all.  For example, if you agree to terms that obligate you\nto collect a royalty for further conveying from those to whom you convey\nthe Program, the only way you could satisfy both those terms and this\nLicense would be to refrain entirely from conveying the Program.\n\n  13. Use with the GNU Affero General Public License.\n\n  Notwithstanding any other provision of this License, you have\npermission to link or combine any covered work with a work licensed\nunder version 3 of the GNU Affero General Public License into a single\ncombined work, and to convey the resulting work.  The terms of this\nLicense will continue to apply to the part which is the covered work,\nbut the special requirements of the GNU Affero General Public License,\nsection 13, concerning interaction through a network will apply to the\ncombination as such.\n\n  14. Revised Versions of this License.\n\n  The Free Software Foundation may publish revised and/or new versions of\nthe GNU General Public License from time to time.  Such new versions will\nbe similar in spirit to the present version, but may differ in detail to\naddress new problems or concerns.\n\n  Each version is given a distinguishing version number.  If the\nProgram specifies that a certain numbered version of the GNU General\nPublic License \"or any later version\" applies to it, you have the\noption of following the terms and conditions either of that numbered\nversion or of any later version published by the Free Software\nFoundation.  If the Program does not specify a version number of the\nGNU General Public License, you may choose any version ever published\nby the Free Software Foundation.\n\n  If the Program specifies that a proxy can decide which future\nversions of the GNU General Public License can be used, that proxy's\npublic statement of acceptance of a version permanently authorizes you\nto choose that version for the Program.\n\n  Later license versions may give you additional or different\npermissions.  However, no additional obligations are imposed on any\nauthor or copyright holder as a result of your choosing to follow a\nlater version.\n\n  15. Disclaimer of Warranty.\n\n  THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY\nAPPLICABLE LAW.  EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT\nHOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM \"AS IS\" WITHOUT WARRANTY\nOF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO,\nTHE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR\nPURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM\nIS WITH YOU.  SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF\nALL NECESSARY SERVICING, REPAIR OR CORRECTION.\n\n  16. Limitation of Liability.\n\n  IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING\nWILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS\nTHE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY\nGENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE\nUSE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF\nDATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD\nPARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS),\nEVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF\nSUCH DAMAGES.\n\n  17. Interpretation of Sections 15 and 16.\n\n  If the disclaimer of warranty and limitation of liability provided\nabove cannot be given local legal effect according to their terms,\nreviewing courts shall apply local law that most closely approximates\nan absolute waiver of all civil liability in connection with the\nProgram, unless a warranty or assumption of liability accompanies a\ncopy of the Program in return for a fee.\n\n                     END OF TERMS AND CONDITIONS\n\n            How to Apply These Terms to Your New Programs\n\n  If you develop a new program, and you want it to be of the greatest\npossible use to the public, the best way to achieve this is to make it\nfree software which everyone can redistribute and change under these terms.\n\n  To do so, attach the following notices to the program.  It is safest\nto attach them to the start of each source file to most effectively\nstate the exclusion of warranty; and each file should have at least\nthe \"copyright\" line and a pointer to where the full notice is found.\n\n    <one line to give the program's name and a brief idea of what it does.>\n    Copyright (C) <year>  <name of author>\n\n    This program is free software: you can redistribute it and/or modify\n    it under the terms of the GNU General Public License as published by\n    the Free Software Foundation, either version 3 of the License, or\n    (at your option) any later version.\n\n    This program is distributed in the hope that it will be useful,\n    but WITHOUT ANY WARRANTY; without even the implied warranty of\n    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n    GNU General Public License for more details.\n\n    You should have received a copy of the GNU General Public License\n    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n\nAlso add information on how to contact you by electronic and paper mail.\n\n  If the program does terminal interaction, make it output a short\nnotice like this when it starts in an interactive mode:\n\n    <program>  Copyright (C) <year>  <name of author>\n    This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'.\n    This is free software, and you are welcome to redistribute it\n    under certain conditions; type `show c' for details.\n\nThe hypothetical commands `show w' and `show c' should show the appropriate\nparts of the General Public License.  Of course, your program's commands\nmight be different; for a GUI interface, you would use an \"about box\".\n\n  You should also get your employer (if you work as a programmer) or school,\nif any, to sign a \"copyright disclaimer\" for the program, if necessary.\nFor more information on this, and how to apply and follow the GNU GPL, see\n<https://www.gnu.org/licenses/>.\n\n  The GNU General Public License does not permit incorporating your program\ninto proprietary programs.  If your program is a subroutine library, you\nmay consider it more useful to permit linking proprietary applications with\nthe library.  If this is what you want to do, use the GNU Lesser General\nPublic License instead of this License.  But first, please read\n<https://www.gnu.org/licenses/why-not-lgpl.html>.\n"
  },
  {
    "path": "ingvio_estimator/README.md",
    "content": "# InGVIO\n\nAn invariant filter for visual-inertial-raw GNSS fusion.\n\n**Paper:** InGVIO: A Consistent Invariant Filter For Fast and High-Accuracy GNSS-Visual-Inertial Odometry\n\n**Paper Author:** Changwu Liu, Chen Jiang and Haowen Wang\n\n**Paper Status:** Manuscript submitted to IEEE RA-L for possible publication. Preprint version available on ArXiv.\n\n**Current Paper Link:** https://arxiv.org/abs/2210.15145\n\nInGVIO 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.\n\nMoreover, we offer our fixed-wing datasets in the form of ROS Bags including stereo-visual, IMU and raw-GNSS measurements.\n\n**Fixed-Wing Dataset Link:** https://cloud.tsinghua.edu.cn/d/4fd9b8a81e0f4186a722\n                             password to extract: lcw18_thu_uav\n\nThe 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'.\n\n## 1. System Requirements\n\n### 1.1  Support of C++ 14 Features\n\nThe compiler should at least support c++14 standards.\n\n### 1.2  ROS-Noetic System\n\nInGVIO 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.\n\n### 1.3  Eigen Library\n\nEigen 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.\n\n### 1.4  SuiteSparse Library\n\nWe use [SuiteSparse](https://github.com/DrTimothyAldenDavis/SuiteSparse/releases) Library for sparse QR-decomposition in visual updates. \n\n### 1.5  gnss_comm Library\n\nA 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.\n\n## 2. Build InGVIO\n\nDownload or clone this repo to your ROS workspace.\n\n```\ncd ~/ws_catkin\ncatkin_make\n```\n\nSource the setup file to let ROS recognize the related launch files.\n\n```\nsource devel/setup.bash\n```\n\n## 3. Run with Our Fixed-Wing Datasets\n\nFirst download and unzip our fixed-wing dataset to your environment.\n\nThe 'fw_zed2i_f9p' folder contains the configuration files for fixed-wing datasets. To run InGVIO in your environment, the following should be conducted first.\n\nPlease 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.\n\nIf you want to record the output topics, please set your output dir in 'ingvio/launch/ingvio_mono, stereo_fw_record.launch' files.\n\nTo run with monocular camera, please enter:\n\n```\nroslaunch ingvio_estimator ingvio_mono_fw.launch\n```\n\nFor stereo-case, please enter:\n\n```\nroslaunch ingvio_estimator ingvio_stereo_fw.launch\n```\n\nTo record the output topics, you can directly use our script file by:\n\n```\nroslaunch ingvio_estimator ingvio_mono_fw_record.launch\nroslaunch ingvio_estimator ingvio_stereo_fw_record.launch\n```\n\nRviz will be automatically launched. Run the ROS Bag to see the results:\n\n```\nrosbag play fw_easy.bag --pause\n```\n\n## 4. Run with GVINS Public Datasets\n\n### 4.1 Configuration Settings\n\nThe 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.\n\nTo run InGVIO in your environment, the following should be conducted first.\n\nPlease 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.\n\nIf you want to record the output topics, please set your output dir in 'ingvio/launch/ingvio_mono, stereo_sf_record.launch' files.\n\nTo run with monocular camera, please enter:\n\n```\nroslaunch ingvio_estimator ingvio_mono_sf.launch\n```\n\nFor stereo-case, please enter:\n\n```\nroslaunch ingvio_estimator ingvio_stereo_sf.launch\n```\n\nTo record the output topics, you can directly use our script file by:\n\n```\nroslaunch ingvio_estimator ingvio_mono_fw_record.launch\nroslaunch ingvio_estimator ingvio_stereo_fw_record.launch\n```\n\nRviz will be automatically launched. Run the ROS Bag to see the results:\n\n```\nrosbag play sports_field.bag --pause\nrosbag play indoors_outdoors.bag --pause\nrosbag play urban_driving.bag --pause\n```\n\n### 4.2 Parameter Tuning Hints\n\n**For sports_field.bag:** Monocular/Stereo Case: max_pts_frame = 150/110, visual_noise = 0.12, gnss_chi2_test = false, imu_buffer_size = 3000.\n\n**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.\n\n**For indoors_outdoors.bag:** Monocular/Stereo Case: max_pts_frame = 150/110, visual_noise = 0.18, gnss_chi2_test = false, gnss_strong_reject = true. \n\nPlease modify or try other parameters if the above behave not well. Good parameters may be different under different environment settings.\n\n## 5. Acknowledgements\n\nThe 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.\n\nThe [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.\n\n## 6. License\n\nThis software is open-sourced under GPLv3 license. See [GNU GPL v3.0 - GNU](https://www.gnu.org/licenses/gpl-3.0.html).\n\nPlease cite our paper if you use either our code or our fixed-wing datasets.\n"
  },
  {
    "path": "ingvio_estimator/cmake/FindSuiteSparse.cmake",
    "content": "# Ceres Solver - A fast non-linear least squares minimizer\n# Copyright 2015 Google Inc. All rights reserved.\n# http://ceres-solver.org/\n#\n# Redistribution and use in source and binary forms, with or without\n# modification, are permitted provided that the following conditions are met:\n#\n# * Redistributions of source code must retain the above copyright notice,\n#   this list of conditions and the following disclaimer.\n# * Redistributions in binary form must reproduce the above copyright notice,\n#   this list of conditions and the following disclaimer in the documentation\n#   and/or other materials provided with the distribution.\n# * Neither the name of Google Inc. nor the names of its contributors may be\n#   used to endorse or promote products derived from this software without\n#   specific prior written permission.\n#\n# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"\n# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\n# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE\n# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE\n# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR\n# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF\n# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS\n# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN\n# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)\n# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n# POSSIBILITY OF SUCH DAMAGE.\n#\n# Author: alexs.mac@gmail.com (Alex Stewart)\n#\n\n# FindSuiteSparse.cmake - Find SuiteSparse libraries & dependencies.\n#\n# This module defines the following variables:\n#\n# SUITESPARSE_FOUND: TRUE iff SuiteSparse and all dependencies have been found.\n# SUITESPARSE_INCLUDE_DIRS: Include directories for all SuiteSparse components.\n# SUITESPARSE_LIBRARIES: Libraries for all SuiteSparse component libraries and\n#                        dependencies.\n# SUITESPARSE_VERSION: Extracted from UFconfig.h (<= v3) or\n#                      SuiteSparse_config.h (>= v4).\n# SUITESPARSE_MAIN_VERSION: Equal to 4 if SUITESPARSE_VERSION = 4.2.1\n# SUITESPARSE_SUB_VERSION: Equal to 2 if SUITESPARSE_VERSION = 4.2.1\n# SUITESPARSE_SUBSUB_VERSION: Equal to 1 if SUITESPARSE_VERSION = 4.2.1\n#\n# SUITESPARSE_IS_BROKEN_SHARED_LINKING_UBUNTU_SYSTEM_VERSION: TRUE iff running\n#     on Ubuntu, SUITESPARSE_VERSION is 3.4.0 and found SuiteSparse is a system\n#     install, in which case found version of SuiteSparse cannot be used to link\n#     a shared library due to a bug (static linking is unaffected).\n#\n# The following variables control the behaviour of this module:\n#\n# SUITESPARSE_INCLUDE_DIR_HINTS: List of additional directories in which to\n#                                search for SuiteSparse includes,\n#                                e.g: /timbuktu/include.\n# SUITESPARSE_LIBRARY_DIR_HINTS: List of additional directories in which to\n#                                search for SuiteSparse libraries,\n#                                e.g: /timbuktu/lib.\n#\n# The following variables define the presence / includes & libraries for the\n# SuiteSparse components searched for, the SUITESPARSE_XX variables are the\n# union of the variables for all components.\n#\n# == Symmetric Approximate Minimum Degree (AMD)\n# AMD_FOUND\n# AMD_INCLUDE_DIR\n# AMD_LIBRARY\n#\n# == Constrained Approximate Minimum Degree (CAMD)\n# CAMD_FOUND\n# CAMD_INCLUDE_DIR\n# CAMD_LIBRARY\n#\n# == Column Approximate Minimum Degree (COLAMD)\n# COLAMD_FOUND\n# COLAMD_INCLUDE_DIR\n# COLAMD_LIBRARY\n#\n# Constrained Column Approximate Minimum Degree (CCOLAMD)\n# CCOLAMD_FOUND\n# CCOLAMD_INCLUDE_DIR\n# CCOLAMD_LIBRARY\n#\n# == Sparse Supernodal Cholesky Factorization and Update/Downdate (CHOLMOD)\n# CHOLMOD_FOUND\n# CHOLMOD_INCLUDE_DIR\n# CHOLMOD_LIBRARY\n#\n# == Multifrontal Sparse QR (SuiteSparseQR)\n# SUITESPARSEQR_FOUND\n# SUITESPARSEQR_INCLUDE_DIR\n# SUITESPARSEQR_LIBRARY\n#\n# == Common configuration for all but CSparse (SuiteSparse version >= 4).\n# SUITESPARSE_CONFIG_FOUND\n# SUITESPARSE_CONFIG_INCLUDE_DIR\n# SUITESPARSE_CONFIG_LIBRARY\n#\n# == Common configuration for all but CSparse (SuiteSparse version < 4).\n# UFCONFIG_FOUND\n# UFCONFIG_INCLUDE_DIR\n#\n# Optional SuiteSparse Dependencies:\n#\n# == Serial Graph Partitioning and Fill-reducing Matrix Ordering (METIS)\n# METIS_FOUND\n# METIS_LIBRARY\n#\n# == Intel Thread Building Blocks (TBB)\n# TBB_FOUND\n# TBB_LIBRARY\n# TBB_MALLOC_FOUND\n# TBB_MALLOC_LIBRARY\n\n# Reset CALLERS_CMAKE_FIND_LIBRARY_PREFIXES to its value when\n# FindSuiteSparse was invoked.\nmacro(SUITESPARSE_RESET_FIND_LIBRARY_PREFIX)\n  if (MSVC)\n    set(CMAKE_FIND_LIBRARY_PREFIXES \"${CALLERS_CMAKE_FIND_LIBRARY_PREFIXES}\")\n  endif (MSVC)\nendmacro(SUITESPARSE_RESET_FIND_LIBRARY_PREFIX)\n\n# Called if we failed to find SuiteSparse or any of it's required dependencies,\n# unsets all public (designed to be used externally) variables and reports\n# error message at priority depending upon [REQUIRED/QUIET/<NONE>] argument.\nmacro(SUITESPARSE_REPORT_NOT_FOUND REASON_MSG)\n  unset(SUITESPARSE_FOUND)\n  unset(SUITESPARSE_INCLUDE_DIRS)\n  unset(SUITESPARSE_LIBRARIES)\n  unset(SUITESPARSE_VERSION)\n  unset(SUITESPARSE_MAIN_VERSION)\n  unset(SUITESPARSE_SUB_VERSION)\n  unset(SUITESPARSE_SUBSUB_VERSION)\n  # Do NOT unset SUITESPARSE_FOUND_REQUIRED_VARS here, as it is used by\n  # FindPackageHandleStandardArgs() to generate the automatic error message on\n  # failure which highlights which components are missing.\n\n  suitesparse_reset_find_library_prefix()\n\n  # Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by FindPackage()\n  # use the camelcase library name, not uppercase.\n  if (SuiteSparse_FIND_QUIETLY)\n    message(STATUS \"Failed to find SuiteSparse - \" ${REASON_MSG} ${ARGN})\n  elseif (SuiteSparse_FIND_REQUIRED)\n    message(FATAL_ERROR \"Failed to find SuiteSparse - \" ${REASON_MSG} ${ARGN})\n  else()\n    # Neither QUIETLY nor REQUIRED, use no priority which emits a message\n    # but continues configuration and allows generation.\n    message(\"-- Failed to find SuiteSparse - \" ${REASON_MSG} ${ARGN})\n  endif (SuiteSparse_FIND_QUIETLY)\n\n  # Do not call return(), s/t we keep processing if not called with REQUIRED\n  # and report all missing components, rather than bailing after failing to find\n  # the first.\nendmacro(SUITESPARSE_REPORT_NOT_FOUND)\n\n# Protect against any alternative find_package scripts for this library having\n# been called previously (in a client project) which set SUITESPARSE_FOUND, but\n# not the other variables we require / set here which could cause the search\n# logic here to fail.\nunset(SUITESPARSE_FOUND)\n\n# Handle possible presence of lib prefix for libraries on MSVC, see\n# also SUITESPARSE_RESET_FIND_LIBRARY_PREFIX().\nif (MSVC)\n  # Preserve the caller's original values for CMAKE_FIND_LIBRARY_PREFIXES\n  # s/t we can set it back before returning.\n  set(CALLERS_CMAKE_FIND_LIBRARY_PREFIXES \"${CMAKE_FIND_LIBRARY_PREFIXES}\")\n  # The empty string in this list is important, it represents the case when\n  # the libraries have no prefix (shared libraries / DLLs).\n  set(CMAKE_FIND_LIBRARY_PREFIXES \"lib\" \"\" \"${CMAKE_FIND_LIBRARY_PREFIXES}\")\nendif (MSVC)\n\n# Specify search directories for include files and libraries (this is the union\n# of the search directories for all OSs).  Search user-specified hint\n# directories first if supplied, and search user-installed locations first\n# so that we prefer user installs to system installs where both exist.\nlist(APPEND SUITESPARSE_CHECK_INCLUDE_DIRS\n  /opt/local/include\n  /opt/local/include/ufsparse # Mac OS X\n  /usr/local/homebrew/include # Mac OS X\n  /usr/local/include\n  /usr/include)\nlist(APPEND SUITESPARSE_CHECK_LIBRARY_DIRS\n  /opt/local/lib\n  /opt/local/lib/ufsparse # Mac OS X\n  /usr/local/homebrew/lib # Mac OS X\n  /usr/local/lib\n  /usr/lib)\n# Additional suffixes to try appending to each search path.\nlist(APPEND SUITESPARSE_CHECK_PATH_SUFFIXES\n  suitesparse) # Windows/Ubuntu\n\n# Wrappers to find_path/library that pass the SuiteSparse search hints/paths.\n#\n# suitesparse_find_component(<component> [FILES name1 [name2 ...]]\n#                                        [LIBRARIES name1 [name2 ...]]\n#                                        [REQUIRED])\nmacro(suitesparse_find_component COMPONENT)\n  include(CMakeParseArguments)\n  set(OPTIONS REQUIRED)\n  set(MULTI_VALUE_ARGS FILES LIBRARIES)\n  cmake_parse_arguments(SUITESPARSE_FIND_${COMPONENT}\n    \"${OPTIONS}\" \"\" \"${MULTI_VALUE_ARGS}\" ${ARGN})\n\n  if (SUITESPARSE_FIND_${COMPONENT}_REQUIRED)\n    list(APPEND SUITESPARSE_FOUND_REQUIRED_VARS ${COMPONENT}_FOUND)\n  endif()\n\n  set(${COMPONENT}_FOUND TRUE)\n  if (SUITESPARSE_FIND_${COMPONENT}_FILES)\n    find_path(${COMPONENT}_INCLUDE_DIR\n      NAMES ${SUITESPARSE_FIND_${COMPONENT}_FILES}\n      HINTS ${SUITESPARSE_INCLUDE_DIR_HINTS}\n      PATHS ${SUITESPARSE_CHECK_INCLUDE_DIRS}\n      PATH_SUFFIXES ${SUITESPARSE_CHECK_PATH_SUFFIXES})\n    if (${COMPONENT}_INCLUDE_DIR)\n      message(STATUS \"Found ${COMPONENT} headers in: \"\n        \"${${COMPONENT}_INCLUDE_DIR}\")\n      mark_as_advanced(${COMPONENT}_INCLUDE_DIR)\n    else()\n      # Specified headers not found.\n      set(${COMPONENT}_FOUND FALSE)\n      if (SUITESPARSE_FIND_${COMPONENT}_REQUIRED)\n        suitesparse_report_not_found(\n          \"Did not find ${COMPONENT} header (required SuiteSparse component).\")\n      else()\n        message(STATUS \"Did not find ${COMPONENT} header (optional \"\n          \"SuiteSparse component).\")\n        # Hide optional vars from CMake GUI even if not found.\n        mark_as_advanced(${COMPONENT}_INCLUDE_DIR)\n      endif()\n    endif()\n  endif()\n\n  if (SUITESPARSE_FIND_${COMPONENT}_LIBRARIES)\n    find_library(${COMPONENT}_LIBRARY\n      NAMES ${SUITESPARSE_FIND_${COMPONENT}_LIBRARIES}\n      HINTS ${SUITESPARSE_LIBRARY_DIR_HINTS}\n      PATHS ${SUITESPARSE_CHECK_LIBRARY_DIRS}\n      PATH_SUFFIXES ${SUITESPARSE_CHECK_PATH_SUFFIXES})\n    if (${COMPONENT}_LIBRARY)\n      message(STATUS \"Found ${COMPONENT} library: ${${COMPONENT}_LIBRARY}\")\n      mark_as_advanced(${COMPONENT}_LIBRARY)\n    else ()\n      # Specified libraries not found.\n      set(${COMPONENT}_FOUND FALSE)\n      if (SUITESPARSE_FIND_${COMPONENT}_REQUIRED)\n        suitesparse_report_not_found(\n          \"Did not find ${COMPONENT} library (required SuiteSparse component).\")\n      else()\n        message(STATUS \"Did not find ${COMPONENT} library (optional SuiteSparse \"\n          \"dependency)\")\n        # Hide optional vars from CMake GUI even if not found.\n        mark_as_advanced(${COMPONENT}_LIBRARY)\n      endif()\n    endif()\n  endif()\nendmacro()\n\n# Given the number of components of SuiteSparse, and to ensure that the\n# automatic failure message generated by FindPackageHandleStandardArgs()\n# when not all required components are found is helpful, we maintain a list\n# of all variables that must be defined for SuiteSparse to be considered found.\nunset(SUITESPARSE_FOUND_REQUIRED_VARS)\n\n# BLAS.\nfind_package(BLAS QUIET)\nif (NOT BLAS_FOUND)\n  suitesparse_report_not_found(\n    \"Did not find BLAS library (required for SuiteSparse).\")\nendif (NOT BLAS_FOUND)\nlist(APPEND SUITESPARSE_FOUND_REQUIRED_VARS BLAS_FOUND)\n\n# LAPACK.\nfind_package(LAPACK QUIET)\nif (NOT LAPACK_FOUND)\n  suitesparse_report_not_found(\n    \"Did not find LAPACK library (required for SuiteSparse).\")\nendif (NOT LAPACK_FOUND)\nlist(APPEND SUITESPARSE_FOUND_REQUIRED_VARS LAPACK_FOUND)\n\nsuitesparse_find_component(AMD REQUIRED FILES amd.h LIBRARIES amd)\nsuitesparse_find_component(CAMD REQUIRED FILES camd.h LIBRARIES camd)\nsuitesparse_find_component(COLAMD REQUIRED FILES colamd.h LIBRARIES colamd)\nsuitesparse_find_component(CCOLAMD REQUIRED FILES ccolamd.h LIBRARIES ccolamd)\nsuitesparse_find_component(CHOLMOD REQUIRED FILES cholmod.h LIBRARIES cholmod)\nsuitesparse_find_component(\n  SUITESPARSEQR REQUIRED FILES SuiteSparseQR.hpp LIBRARIES spqr)\nif (SUITESPARSEQR_FOUND)\n  # SuiteSparseQR may be compiled with Intel Threading Building Blocks,\n  # we assume that if TBB is installed, SuiteSparseQR was compiled with\n  # support for it, this will do no harm if it wasn't.\n  find_package(TBB QUIET)\n  if (TBB_FOUND)\n    message(STATUS \"Found Intel Thread Building Blocks (TBB) library \"\n      \"(${TBB_VERSION}) assuming SuiteSparseQR was compiled \"\n      \"with TBB.\")\n    # Add the TBB libraries to the SuiteSparseQR libraries (the only\n    # libraries to optionally depend on TBB).\n    list(APPEND SUITESPARSEQR_LIBRARY ${TBB_LIBRARIES})\n  else()\n    message(STATUS \"Did not find Intel TBB library, assuming SuiteSparseQR was \"\n      \"not compiled with TBB.\")\n  endif()\nendif(SUITESPARSEQR_FOUND)\n\n# UFconfig / SuiteSparse_config.\n#\n# If SuiteSparse version is >= 4 then SuiteSparse_config is required.\n# For SuiteSparse 3, UFconfig.h is required.\nsuitesparse_find_component(\n  SUITESPARSE_CONFIG FILES SuiteSparse_config.h LIBRARIES suitesparseconfig)\n\nif (SUITESPARSE_CONFIG_FOUND)\n  # SuiteSparse_config (SuiteSparse version >= 4) requires librt library for\n  # timing by default when compiled on Linux or Unix, but not on OSX (which\n  # does not have librt).\n  if (CMAKE_SYSTEM_NAME MATCHES \"Linux\" OR UNIX AND NOT APPLE)\n    suitesparse_find_component(LIBRT LIBRARIES rt)\n    if (LIBRT_FOUND)\n      message(STATUS \"Adding librt: ${LIBRT_LIBRARY} to \"\n        \"SuiteSparse_config libraries (required on Linux & Unix [not OSX] if \"\n        \"SuiteSparse is compiled with timing).\")\n      list(APPEND SUITESPARSE_CONFIG_LIBRARY ${LIBRT_LIBRARY})\n    else()\n      message(STATUS \"Could not find librt, but found SuiteSparse_config, \"\n        \"assuming that SuiteSparse was compiled without timing.\")\n    endif ()\n  endif (CMAKE_SYSTEM_NAME MATCHES \"Linux\" OR UNIX AND NOT APPLE)\nelse()\n  # Failed to find SuiteSparse_config (>= v4 installs), instead look for\n  # UFconfig header which should be present in < v4 installs.\n  suitesparse_find_component(UFCONFIG FILES UFconfig.h)\nendif ()\n\nif (NOT SUITESPARSE_CONFIG_FOUND AND\n    NOT UFCONFIG_FOUND)\n  suitesparse_report_not_found(\n    \"Failed to find either: SuiteSparse_config header & library (should be \"\n    \"present in all SuiteSparse >= v4 installs), or UFconfig header (should \"\n    \"be present in all SuiteSparse < v4 installs).\")\nendif()\n\n# Extract the SuiteSparse version from the appropriate header (UFconfig.h for\n# <= v3, SuiteSparse_config.h for >= v4).\nlist(APPEND SUITESPARSE_FOUND_REQUIRED_VARS SUITESPARSE_VERSION)\n\nif (UFCONFIG_FOUND)\n  # SuiteSparse version <= 3.\n  set(SUITESPARSE_VERSION_FILE ${UFCONFIG_INCLUDE_DIR}/UFconfig.h)\n  if (NOT EXISTS ${SUITESPARSE_VERSION_FILE})\n    suitesparse_report_not_found(\n      \"Could not find file: ${SUITESPARSE_VERSION_FILE} containing version \"\n      \"information for <= v3 SuiteSparse installs, but UFconfig was found \"\n      \"(only present in <= v3 installs).\")\n  else (NOT EXISTS ${SUITESPARSE_VERSION_FILE})\n    file(READ ${SUITESPARSE_VERSION_FILE} UFCONFIG_CONTENTS)\n\n    string(REGEX MATCH \"#define SUITESPARSE_MAIN_VERSION [0-9]+\"\n      SUITESPARSE_MAIN_VERSION \"${UFCONFIG_CONTENTS}\")\n    string(REGEX REPLACE \"#define SUITESPARSE_MAIN_VERSION ([0-9]+)\" \"\\\\1\"\n      SUITESPARSE_MAIN_VERSION \"${SUITESPARSE_MAIN_VERSION}\")\n\n    string(REGEX MATCH \"#define SUITESPARSE_SUB_VERSION [0-9]+\"\n      SUITESPARSE_SUB_VERSION \"${UFCONFIG_CONTENTS}\")\n    string(REGEX REPLACE \"#define SUITESPARSE_SUB_VERSION ([0-9]+)\" \"\\\\1\"\n      SUITESPARSE_SUB_VERSION \"${SUITESPARSE_SUB_VERSION}\")\n\n    string(REGEX MATCH \"#define SUITESPARSE_SUBSUB_VERSION [0-9]+\"\n      SUITESPARSE_SUBSUB_VERSION \"${UFCONFIG_CONTENTS}\")\n    string(REGEX REPLACE \"#define SUITESPARSE_SUBSUB_VERSION ([0-9]+)\" \"\\\\1\"\n      SUITESPARSE_SUBSUB_VERSION \"${SUITESPARSE_SUBSUB_VERSION}\")\n\n    # This is on a single line s/t CMake does not interpret it as a list of\n    # elements and insert ';' separators which would result in 4.;2.;1 nonsense.\n    set(SUITESPARSE_VERSION\n      \"${SUITESPARSE_MAIN_VERSION}.${SUITESPARSE_SUB_VERSION}.${SUITESPARSE_SUBSUB_VERSION}\")\n  endif (NOT EXISTS ${SUITESPARSE_VERSION_FILE})\nendif (UFCONFIG_FOUND)\n\nif (SUITESPARSE_CONFIG_FOUND)\n  # SuiteSparse version >= 4.\n  set(SUITESPARSE_VERSION_FILE\n    ${SUITESPARSE_CONFIG_INCLUDE_DIR}/SuiteSparse_config.h)\n  if (NOT EXISTS ${SUITESPARSE_VERSION_FILE})\n    suitesparse_report_not_found(\n      \"Could not find file: ${SUITESPARSE_VERSION_FILE} containing version \"\n      \"information for >= v4 SuiteSparse installs, but SuiteSparse_config was \"\n      \"found (only present in >= v4 installs).\")\n  else (NOT EXISTS ${SUITESPARSE_VERSION_FILE})\n    file(READ ${SUITESPARSE_VERSION_FILE} SUITESPARSE_CONFIG_CONTENTS)\n\n    string(REGEX MATCH \"#define SUITESPARSE_MAIN_VERSION [0-9]+\"\n      SUITESPARSE_MAIN_VERSION \"${SUITESPARSE_CONFIG_CONTENTS}\")\n    string(REGEX REPLACE \"#define SUITESPARSE_MAIN_VERSION ([0-9]+)\" \"\\\\1\"\n      SUITESPARSE_MAIN_VERSION \"${SUITESPARSE_MAIN_VERSION}\")\n\n    string(REGEX MATCH \"#define SUITESPARSE_SUB_VERSION [0-9]+\"\n      SUITESPARSE_SUB_VERSION \"${SUITESPARSE_CONFIG_CONTENTS}\")\n    string(REGEX REPLACE \"#define SUITESPARSE_SUB_VERSION ([0-9]+)\" \"\\\\1\"\n      SUITESPARSE_SUB_VERSION \"${SUITESPARSE_SUB_VERSION}\")\n\n    string(REGEX MATCH \"#define SUITESPARSE_SUBSUB_VERSION [0-9]+\"\n      SUITESPARSE_SUBSUB_VERSION \"${SUITESPARSE_CONFIG_CONTENTS}\")\n    string(REGEX REPLACE \"#define SUITESPARSE_SUBSUB_VERSION ([0-9]+)\" \"\\\\1\"\n      SUITESPARSE_SUBSUB_VERSION \"${SUITESPARSE_SUBSUB_VERSION}\")\n\n    # This is on a single line s/t CMake does not interpret it as a list of\n    # elements and insert ';' separators which would result in 4.;2.;1 nonsense.\n    set(SUITESPARSE_VERSION\n      \"${SUITESPARSE_MAIN_VERSION}.${SUITESPARSE_SUB_VERSION}.${SUITESPARSE_SUBSUB_VERSION}\")\n  endif (NOT EXISTS ${SUITESPARSE_VERSION_FILE})\nendif (SUITESPARSE_CONFIG_FOUND)\n\n# METIS (Optional dependency).\nsuitesparse_find_component(METIS LIBRARIES metis)\n\n# Only mark SuiteSparse as found if all required components and dependencies\n# have been found.\nset(SUITESPARSE_FOUND TRUE)\nforeach(REQUIRED_VAR ${SUITESPARSE_FOUND_REQUIRED_VARS})\n  if (NOT ${REQUIRED_VAR})\n    set(SUITESPARSE_FOUND FALSE)\n  endif (NOT ${REQUIRED_VAR})\nendforeach(REQUIRED_VAR ${SUITESPARSE_FOUND_REQUIRED_VARS})\n\nif (SUITESPARSE_FOUND)\n  list(APPEND SUITESPARSE_INCLUDE_DIRS\n    ${AMD_INCLUDE_DIR}\n    ${CAMD_INCLUDE_DIR}\n    ${COLAMD_INCLUDE_DIR}\n    ${CCOLAMD_INCLUDE_DIR}\n    ${CHOLMOD_INCLUDE_DIR}\n    ${SUITESPARSEQR_INCLUDE_DIR})\n  # Handle config separately, as otherwise at least one of them will be set\n  # to NOTFOUND which would cause any check on SUITESPARSE_INCLUDE_DIRS to fail.\n  if (SUITESPARSE_CONFIG_FOUND)\n    list(APPEND SUITESPARSE_INCLUDE_DIRS\n      ${SUITESPARSE_CONFIG_INCLUDE_DIR})\n  endif (SUITESPARSE_CONFIG_FOUND)\n  if (UFCONFIG_FOUND)\n    list(APPEND SUITESPARSE_INCLUDE_DIRS\n      ${UFCONFIG_INCLUDE_DIR})\n  endif (UFCONFIG_FOUND)\n  # As SuiteSparse includes are often all in the same directory, remove any\n  # repetitions.\n  list(REMOVE_DUPLICATES SUITESPARSE_INCLUDE_DIRS)\n\n  # Important: The ordering of these libraries is *NOT* arbitrary, as these\n  # could potentially be static libraries their link ordering is important.\n  list(APPEND SUITESPARSE_LIBRARIES\n    ${SUITESPARSEQR_LIBRARY}\n    ${CHOLMOD_LIBRARY}\n    ${CCOLAMD_LIBRARY}\n    ${CAMD_LIBRARY}\n    ${COLAMD_LIBRARY}\n    ${AMD_LIBRARY}\n    ${LAPACK_LIBRARIES}\n    ${BLAS_LIBRARIES})\n  if (SUITESPARSE_CONFIG_FOUND)\n    list(APPEND SUITESPARSE_LIBRARIES\n      ${SUITESPARSE_CONFIG_LIBRARY})\n  endif (SUITESPARSE_CONFIG_FOUND)\n  if (METIS_FOUND)\n    list(APPEND SUITESPARSE_LIBRARIES\n      ${METIS_LIBRARY})\n  endif (METIS_FOUND)\nendif()\n\n# Determine if we are running on Ubuntu with the package install of SuiteSparse\n# which is broken and does not support linking a shared library.\nset(SUITESPARSE_IS_BROKEN_SHARED_LINKING_UBUNTU_SYSTEM_VERSION FALSE)\nif (CMAKE_SYSTEM_NAME MATCHES \"Linux\" AND\n    SUITESPARSE_VERSION VERSION_EQUAL 3.4.0)\n  find_program(LSB_RELEASE_EXECUTABLE lsb_release)\n  if (LSB_RELEASE_EXECUTABLE)\n    # Any even moderately recent Ubuntu release (likely to be affected by\n    # this bug) should have lsb_release, if it isn't present we are likely\n    # on a different Linux distribution (should be fine).\n    execute_process(COMMAND ${LSB_RELEASE_EXECUTABLE} -si\n      OUTPUT_VARIABLE LSB_DISTRIBUTOR_ID\n      OUTPUT_STRIP_TRAILING_WHITESPACE)\n\n    if (LSB_DISTRIBUTOR_ID MATCHES \"Ubuntu\" AND\n        SUITESPARSE_LIBRARIES MATCHES \"/usr/lib/libamd\")\n      # We are on Ubuntu, and the SuiteSparse version matches the broken\n      # system install version and is a system install.\n      set(SUITESPARSE_IS_BROKEN_SHARED_LINKING_UBUNTU_SYSTEM_VERSION TRUE)\n      message(STATUS \"Found system install of SuiteSparse \"\n        \"${SUITESPARSE_VERSION} running on Ubuntu, which has a known bug \"\n        \"preventing linking of shared libraries (static linking unaffected).\")\n    endif (LSB_DISTRIBUTOR_ID MATCHES \"Ubuntu\" AND\n      SUITESPARSE_LIBRARIES MATCHES \"/usr/lib/libamd\")\n  endif (LSB_RELEASE_EXECUTABLE)\nendif (CMAKE_SYSTEM_NAME MATCHES \"Linux\" AND\n  SUITESPARSE_VERSION VERSION_EQUAL 3.4.0)\n\nsuitesparse_reset_find_library_prefix()\n\n# Handle REQUIRED and QUIET arguments to FIND_PACKAGE\ninclude(FindPackageHandleStandardArgs)\nif (SUITESPARSE_FOUND)\n  find_package_handle_standard_args(SuiteSparse\n    REQUIRED_VARS ${SUITESPARSE_FOUND_REQUIRED_VARS}\n    VERSION_VAR SUITESPARSE_VERSION\n    FAIL_MESSAGE \"Failed to find some/all required components of SuiteSparse.\")\nelse (SUITESPARSE_FOUND)\n  # Do not pass VERSION_VAR to FindPackageHandleStandardArgs() if we failed to\n  # find SuiteSparse to avoid a confusing autogenerated failure message\n  # that states 'not found (missing: FOO) (found version: x.y.z)'.\n  find_package_handle_standard_args(SuiteSparse\n    REQUIRED_VARS ${SUITESPARSE_FOUND_REQUIRED_VARS}\n    FAIL_MESSAGE \"Failed to find some/all required components of SuiteSparse.\")\nendif (SUITESPARSE_FOUND)\n"
  },
  {
    "path": "ingvio_estimator/cmake/ROS1.cmake",
    "content": "cmake_minimum_required(VERSION 3.16)\n\nfind_package(catkin QUIET COMPONENTS\n    roscpp\n    roslib\n    std_msgs\n    message_filters\n    sensor_msgs\n    eigen_conversions\n    geometry_msgs\n    tf\n    tf_conversions\n    nav_msgs\n    gnss_comm\n)\n\ncatkin_package(\n    CATKIN_DEPENDS roscpp roslib std_msgs message_filters sensor_msgs eigen_conversions geometry_msgs tf tf_conversions nav_msgs gnss_comm\n    INCLUDE_DIRS src/\n    DEPENDS OpenCV SUITESPARSE Boost\n)\n\ninclude_directories(\n    src\n    test\n    ${EIGEN3_INCLUDE_DIR}\n    ${Boost_INCLUDE_DIRS}\n    ${catkin_INCLUDE_DIRS}\n    ${SUITESPARSE_INCLUDE_DIRS}\n)\n\nadd_executable(ingvio \n    ${PROJECT_SOURCE_DIR}/src/AuxGammaFunc.cpp\n    ${PROJECT_SOURCE_DIR}/src/VecState.cpp\n    ${PROJECT_SOURCE_DIR}/src/PoseState.cpp\n    ${PROJECT_SOURCE_DIR}/src/AnchoredLandmark.cpp\n    ${PROJECT_SOURCE_DIR}/src/ImuPropagator.cpp\n    ${PROJECT_SOURCE_DIR}/src/IngvioFilter.cpp\n    ${PROJECT_SOURCE_DIR}/src/IngvioNode.cpp\n    ${PROJECT_SOURCE_DIR}/src/IngvioParams.cpp\n    ${PROJECT_SOURCE_DIR}/src/MapServer.cpp\n    ${PROJECT_SOURCE_DIR}/src/MapServerManager.cpp\n    ${PROJECT_SOURCE_DIR}/src/RemoveLostUpdate.cpp\n    ${PROJECT_SOURCE_DIR}/src/State.cpp\n    ${PROJECT_SOURCE_DIR}/src/StateManager.cpp\n    ${PROJECT_SOURCE_DIR}/src/LandmarkUpdate.cpp\n    ${PROJECT_SOURCE_DIR}/src/SwMargUpdate.cpp\n    ${PROJECT_SOURCE_DIR}/src/KeyframeUpdate.cpp\n    ${PROJECT_SOURCE_DIR}/src/Triangulator.cpp\n    ${PROJECT_SOURCE_DIR}/src/Update.cpp\n    ${PROJECT_SOURCE_DIR}/src/GnssProcessor.cpp\n    ${PROJECT_SOURCE_DIR}/src/GnssSync.cpp\n    ${PROJECT_SOURCE_DIR}/src/GvioAligner.cpp\n    ${PROJECT_SOURCE_DIR}/src/GnssUpdate.cpp\n    ${PROJECT_SOURCE_DIR}/src/GnssManager.cpp\n    ${PROJECT_SOURCE_DIR}/src/GnssData.cpp\n)\n\nlist(APPEND thirdparty_libs \n    ${catkin_LIBRARIES}\n    ${Boost_LIBRARIES}\n    ${OpenCV_LIBRARIES}\n    ${SUITESPARSE_LIBRARIES}\n)\ntarget_link_libraries(ingvio\n    ${thirdparty_libs}\n)\n\nmessage(STATUS \"Tests enabled! Build in-project tests!\")\n\nfind_package(GTest REQUIRED)\ninclude_directories(${GTEST_INCLUDE_DIRS})\n    \nadd_executable(test_state_manager\n    ${PROJECT_SOURCE_DIR}/src/VecState.cpp\n    ${PROJECT_SOURCE_DIR}/src/PoseState.cpp\n    ${PROJECT_SOURCE_DIR}/test/TestStateManager.cpp\n    ${PROJECT_SOURCE_DIR}/src/AnchoredLandmark.cpp\n    ${PROJECT_SOURCE_DIR}/src/AuxGammaFunc.cpp\n    ${PROJECT_SOURCE_DIR}/src/State.cpp\n    ${PROJECT_SOURCE_DIR}/src/StateManager.cpp\n    ${PROJECT_SOURCE_DIR}/src/IngvioParams.cpp\n)\ntarget_link_libraries(test_state_manager ${GTEST_BOTH_LIBRARIES} pthread ${thirdparty_libs})\ngtest_discover_tests(test_state_manager)\n\nadd_executable(test_propagator\n    ${PROJECT_SOURCE_DIR}/test/TestPropagator.cpp\n    ${PROJECT_SOURCE_DIR}/src/VecState.cpp\n    ${PROJECT_SOURCE_DIR}/src/PoseState.cpp\n    ${PROJECT_SOURCE_DIR}/src/AnchoredLandmark.cpp\n    ${PROJECT_SOURCE_DIR}/src/AuxGammaFunc.cpp\n    ${PROJECT_SOURCE_DIR}/src/State.cpp\n    ${PROJECT_SOURCE_DIR}/src/StateManager.cpp\n    ${PROJECT_SOURCE_DIR}/src/IngvioParams.cpp\n    ${PROJECT_SOURCE_DIR}/src/ImuPropagator.cpp\n)\ntarget_link_libraries(test_propagator ${GTEST_BOTH_LIBRARIES} pthread ${thirdparty_libs})\ngtest_discover_tests(test_propagator)\n\nadd_executable(test_map_server\n    ${PROJECT_SOURCE_DIR}/test/TestMapServer.cpp\n    ${PROJECT_SOURCE_DIR}/src/VecState.cpp\n    ${PROJECT_SOURCE_DIR}/src/PoseState.cpp\n    ${PROJECT_SOURCE_DIR}/test/GenerateNoise.cpp\n    ${PROJECT_SOURCE_DIR}/src/AnchoredLandmark.cpp\n    ${PROJECT_SOURCE_DIR}/src/AuxGammaFunc.cpp\n    ${PROJECT_SOURCE_DIR}/src/State.cpp\n    ${PROJECT_SOURCE_DIR}/src/StateManager.cpp\n    ${PROJECT_SOURCE_DIR}/src/IngvioParams.cpp\n    ${PROJECT_SOURCE_DIR}/src/ImuPropagator.cpp\n    ${PROJECT_SOURCE_DIR}/src/MapServer.cpp\n    ${PROJECT_SOURCE_DIR}/src/MapServerManager.cpp\n    ${PROJECT_SOURCE_DIR}/src/Triangulator.cpp\n)\ntarget_link_libraries(test_map_server ${GTEST_BOTH_LIBRARIES} pthread ${thirdparty_libs})\ngtest_discover_tests(test_map_server)\n\nadd_executable(test_triangulator\n    ${PROJECT_SOURCE_DIR}/test/TestTriangulator.cpp\n    ${PROJECT_SOURCE_DIR}/src/VecState.cpp\n    ${PROJECT_SOURCE_DIR}/src/PoseState.cpp\n    ${PROJECT_SOURCE_DIR}/src/AnchoredLandmark.cpp\n    ${PROJECT_SOURCE_DIR}/test/GenerateNoise.cpp\n    ${PROJECT_SOURCE_DIR}/src/Triangulator.cpp\n    ${PROJECT_SOURCE_DIR}/src/IngvioParams.cpp\n    ${PROJECT_SOURCE_DIR}/src/AuxGammaFunc.cpp\n    ${PROJECT_SOURCE_DIR}/src/MapServer.cpp\n)\ntarget_link_libraries(test_triangulator ${GTEST_BOTH_LIBRARIES} pthread ${thirdparty_libs})\ngtest_discover_tests(test_triangulator)\n"
  },
  {
    "path": "ingvio_estimator/launch/ingvio_mono_fw.launch",
    "content": "<launch>\n    <arg name=\"cam_config_path\" default = \"$(find feature_tracker)/../config/fw_zed2i_f9p/mono_config.yaml\" />\n    \n    <node name=\"mono_tracker\" pkg=\"feature_tracker\" type=\"mono_tracker\" output=\"screen\">\n        <param name=\"config_file\" type=\"string\" value=\"$(arg cam_config_path)\" />\n    </node>\n\n    <arg name=\"ingvio_config_path\" default = \"$(find ingvio_estimator)/../config/fw_zed2i_f9p/ingvio_mono.yaml\" />\n    <node name=\"ingvio_estimator\" pkg=\"ingvio_estimator\" type=\"ingvio\" output=\"screen\">\n        <param name=\"config_file\" type=\"string\" value=\"$(arg ingvio_config_path)\" />\n    </node>\n\n    <node name=\"visualizer\" pkg=\"rviz\" type=\"rviz\" output=\"screen\" args=\"-d $(find ingvio_estimator)/rviz/ingvio_mono.rviz\" />\n</launch>\n"
  },
  {
    "path": "ingvio_estimator/launch/ingvio_mono_fw_record.launch",
    "content": "<launch>\n    <include file = \"$(find ingvio_estimator)/launch/ingvio_mono_fw.launch\" />\n    \n    <arg name = \"is_record_gnss\" default = \"true\" />\n    <arg name = \"recorded_file\" default = \"/home/lcw/VIO/ingvio_dataset_tests/ingvio_fw\" />\n    \n    <node pkg = \"rosbag\" type = \"record\" name = \"data_record_with_gnss\" args = \"record -o $(arg recorded_file) /ingvio_estimator/pose_w /ingvio_estimator/pose_spp /ingvio_estimator/pose_gt\" if = \"$(arg is_record_gnss)\" />\n    \n    <node pkg = \"rosbag\" type = \"record\" name = \"data_record_without_gnss\" args = \"record -o $(arg recorded_file) /ingvio_estimator/pose_w\" unless = \"$(arg is_record_gnss)\" />\n</launch>\n"
  },
  {
    "path": "ingvio_estimator/launch/ingvio_mono_sf.launch",
    "content": "<launch>\n    <arg name=\"cam_config_path\" default = \"$(find feature_tracker)/../config/sportsfield/mono_config.yaml\" />\n    \n    <node name=\"mono_tracker\" pkg=\"feature_tracker\" type=\"mono_tracker\" output=\"screen\">\n        <param name=\"config_file\" type=\"string\" value=\"$(arg cam_config_path)\" />\n    </node>\n\n    <arg name=\"ingvio_config_path\" default = \"$(find ingvio_estimator)/../config/sportsfield/ingvio_mono.yaml\" />\n    <node name=\"ingvio_estimator\" pkg=\"ingvio_estimator\" type=\"ingvio\" output=\"screen\">\n        <param name=\"config_file\" type=\"string\" value=\"$(arg ingvio_config_path)\" />\n    </node>\n\n    <node name=\"visualizer\" pkg=\"rviz\" type=\"rviz\" output=\"screen\" args=\"-d $(find ingvio_estimator)/rviz/ingvio_mono.rviz\" />\n</launch>\n"
  },
  {
    "path": "ingvio_estimator/launch/ingvio_mono_sf_record.launch",
    "content": "<launch>\n    <include file = \"$(find ingvio_estimator)/launch/ingvio_mono_sf.launch\" />\n    \n    <arg name = \"is_record_gnss\" default = \"true\" />\n    <arg name = \"recorded_file\" default = \"/home/lcw/VIO/ingvio_dataset_tests/ingvio\" />\n    \n    <node pkg = \"rosbag\" type = \"record\" name = \"data_record_with_gnss\" args = \"record -o $(arg recorded_file) /ingvio_estimator/pose_w /ingvio_estimator/pose_spp /ingvio_estimator/pose_gt\" if = \"$(arg is_record_gnss)\" />\n    \n    <node pkg = \"rosbag\" type = \"record\" name = \"data_record_without_gnss\" args = \"record -o $(arg recorded_file) /ingvio_estimator/pose_w\" unless = \"$(arg is_record_gnss)\" />\n</launch>\n"
  },
  {
    "path": "ingvio_estimator/launch/ingvio_stereo_fw.launch",
    "content": "<launch>\n    <arg name=\"cam_config_path_left\" default = \"$(find feature_tracker)/../config/fw_zed2i_f9p/stereo_left_config.yaml\" />\n    <arg name=\"cam_config_path_right\" default = \"$(find feature_tracker)/../config/fw_zed2i_f9p/stereo_right_config.yaml\" />\n    \n    <node name=\"stereo_tracker\" pkg=\"feature_tracker\" type=\"stereo_tracker\" output=\"screen\">\n        <param name=\"config_file_left\" type=\"string\" value=\"$(arg cam_config_path_left)\" />\n        <param name=\"config_file_right\" type=\"string\" value=\"$(arg cam_config_path_right)\" />\n    </node>\n\n    <arg name=\"ingvio_config_path\" default = \"$(find ingvio_estimator)/../config/fw_zed2i_f9p/ingvio_stereo.yaml\" />\n    <node name=\"ingvio_estimator\" pkg=\"ingvio_estimator\" type=\"ingvio\" output=\"screen\">\n        <param name=\"config_file\" type=\"string\" value=\"$(arg ingvio_config_path)\" />\n    </node>\n\n    <node name=\"visualizer\" pkg=\"rviz\" type=\"rviz\" output=\"screen\" args=\"-d $(find ingvio_estimator)/rviz/ingvio_stereo.rviz\" />\n</launch>\n"
  },
  {
    "path": "ingvio_estimator/launch/ingvio_stereo_fw_record.launch",
    "content": "<launch>\n    <include file = \"$(find ingvio_estimator)/launch/ingvio_stereo_fw.launch\" />\n    \n    <arg name = \"is_record_gnss\" default = \"false\" />\n    <arg name = \"recorded_file\" default = \"/home/lcw/VIO/ingvio_dataset_tests/ingvio_fw\" />\n    \n    <node pkg = \"rosbag\" type = \"record\" name = \"data_record_with_gnss\" args = \"record -o $(arg recorded_file) /ingvio_estimator/pose_w /ingvio_estimator/pose_spp /ingvio_estimator/pose_gt\" if = \"$(arg is_record_gnss)\" />\n    \n    <node pkg = \"rosbag\" type = \"record\" name = \"data_record_without_gnss\" args = \"record -o $(arg recorded_file) /ingvio_estimator/pose_w\" unless = \"$(arg is_record_gnss)\" />\n</launch>\n"
  },
  {
    "path": "ingvio_estimator/launch/ingvio_stereo_sf.launch",
    "content": "<launch>\n    <arg name=\"cam_config_path_left\" default = \"$(find feature_tracker)/../config/sportsfield/stereo_left_config.yaml\" />\n    <arg name=\"cam_config_path_right\" default = \"$(find feature_tracker)/../config/sportsfield/stereo_right_config.yaml\" />\n    \n    <node name=\"stereo_tracker\" pkg=\"feature_tracker\" type=\"stereo_tracker\" output=\"screen\">\n        <param name=\"config_file_left\" type=\"string\" value=\"$(arg cam_config_path_left)\" />\n        <param name=\"config_file_right\" type=\"string\" value=\"$(arg cam_config_path_right)\" />\n    </node>\n\n    <arg name=\"ingvio_config_path\" default = \"$(find ingvio_estimator)/../config/sportsfield/ingvio_stereo.yaml\" />\n    <node name=\"ingvio_estimator\" pkg=\"ingvio_estimator\" type=\"ingvio\" output=\"screen\">\n        <param name=\"config_file\" type=\"string\" value=\"$(arg ingvio_config_path)\" />\n    </node>\n\n    <node name=\"visualizer\" pkg=\"rviz\" type=\"rviz\" output=\"screen\" args=\"-d $(find ingvio_estimator)/rviz/ingvio_stereo.rviz\" />\n</launch>\n"
  },
  {
    "path": "ingvio_estimator/launch/ingvio_stereo_sf_record.launch",
    "content": "<launch>\n    <include file = \"$(find ingvio_estimator)/launch/ingvio_stereo_sf.launch\" />\n    \n    <arg name = \"is_record_gnss\" default = \"false\" />\n    <arg name = \"recorded_file\" default = \"/home/lcw/VIO/ingvio_dataset_tests/ingvio\" />\n    \n    <node pkg = \"rosbag\" type = \"record\" name = \"data_record_with_gnss\" args = \"record -o $(arg recorded_file) /ingvio_estimator/pose_w /ingvio_estimator/pose_spp /ingvio_estimator/pose_gt\" if = \"$(arg is_record_gnss)\" />\n    \n    <node pkg = \"rosbag\" type = \"record\" name = \"data_record_without_gnss\" args = \"record -o $(arg recorded_file) /ingvio_estimator/pose_w\" unless = \"$(arg is_record_gnss)\" />\n</launch>\n"
  },
  {
    "path": "ingvio_estimator/package.xml",
    "content": "<?xml version=\"1.0\"?>\n<package format=\"3\">\n    \n    <name>ingvio_estimator</name>\n    <version>1.0.0</version>\n    <description>\n        Invariant filter for visual-inertial-GNSS fusion navigation.     \n    </description>\n    \n    <author email=\"cwliu529@163.com\">Changwu Liu</author>\n    <maintainer email=\"cwliu529@163.com\">Changwu Liu</maintainer>\n    \n    <license> GPLv3 </license>\n    \n    <buildtool_depend condition=\"$ROS_VERSION == 1\">catkin</buildtool_depend>\n    <depend condition=\"$ROS_VERSION == 1\">cmake_modules</depend>\n    \n    <depend condition=\"$ROS_VERSION == 1\">roscpp</depend>\n    <depend condition=\"$ROS_VERSION == 1\">roslib</depend>\n    <depend condition=\"$ROS_VERSION == 1\">std_msgs</depend>\n    <depend condition=\"$ROS_VERSION == 1\">message_filters</depend>\n    <depend condition=\"$ROS_VERSION == 1\">sensor_msgs</depend>\n    <depend condition=\"$ROS_VERSION == 1\">eigen_conversions</depend>\n    <depend condition=\"$ROS_VERSION == 1\">geometry_msgs</depend>\n    <depend condition=\"$ROS_VERSION == 1\">tf</depend>\n    <depend condition=\"$ROS_VERSION == 1\">tf_conversions</depend>\n    <depend condition=\"$ROS_VERSION == 1\">nav_msgs</depend>\n    <depend condition=\"$ROS_VERSION == 1\">gnss_comm</depend>\n      \n    <depend condition=\"$ROS_VERSION == 1\">gtest</depend>\n    \n    <export>\n        <build_type condition=\"$ROS_VERSION == 1\">catkin</build_type>    \n    </export>\n    \n</package>\n"
  },
  {
    "path": "ingvio_estimator/rviz/ingvio_mono.rviz",
    "content": "Panels:\n  - Class: rviz/Displays\n    Help Height: 0\n    Name: Displays\n    Property Tree Widget:\n      Expanded:\n        - /Global Options1\n        - /Status1\n      Splitter Ratio: 0.5\n    Tree Height: 301\n  - Class: rviz/Selection\n    Name: Selection\n  - Class: rviz/Tool Properties\n    Expanded:\n      - /2D Pose Estimate1\n      - /2D Nav Goal1\n      - /Publish Point1\n    Name: Tool Properties\n    Splitter Ratio: 0.5886790156364441\n  - Class: rviz/Views\n    Expanded:\n      - /Current View1\n    Name: Views\n    Splitter Ratio: 0.5\n  - Class: rviz/Time\n    Name: Time\n    SyncMode: 0\n    SyncSource: \"\"\nPreferences:\n  PromptSaveOnExit: true\nToolbars:\n  toolButtonStyle: 2\nVisualization Manager:\n  Class: \"\"\n  Displays:\n    - Alpha: 0.5\n      Cell Size: 1\n      Class: rviz/Grid\n      Color: 160; 160; 164\n      Enabled: true\n      Line Style:\n        Line Width: 0.029999999329447746\n        Value: Lines\n      Name: Grid\n      Normal Cell Count: 0\n      Offset:\n        X: 0\n        Y: 0\n        Z: 0\n      Plane: XY\n      Plane Cell Count: 10\n      Reference Frame: <Fixed Frame>\n      Value: true\n    - Class: rviz/Image\n      Enabled: true\n      Image Topic: /mono_tracker/mono_track\n      Max Value: 1\n      Median window: 5\n      Min Value: 0\n      Name: MonoTrack\n      Normalize Range: true\n      Queue Size: 2\n      Transport Hint: raw\n      Unreliable: false\n      Value: true\n    - Alpha: 1\n      Buffer Length: 1\n      Class: rviz/Path\n      Color: 25; 255; 0\n      Enabled: true\n      Head Diameter: 0.30000001192092896\n      Head Length: 0.20000000298023224\n      Length: 0.30000001192092896\n      Line Style: Lines\n      Line Width: 0.029999999329447746\n      Name: Path_world\n      Offset:\n        X: 0\n        Y: 0\n        Z: 0\n      Pose Color: 255; 85; 255\n      Pose Style: None\n      Queue Size: 2\n      Radius: 0.029999999329447746\n      Shaft Diameter: 0.10000000149011612\n      Shaft Length: 0.10000000149011612\n      Topic: /ingvio_estimator/path_w\n      Unreliable: false\n      Value: true\n    - Angle Tolerance: 0.10000000149011612\n      Class: rviz/Odometry\n      Covariance:\n        Orientation:\n          Alpha: 0.5\n          Color: 255; 255; 127\n          Color Style: Unique\n          Frame: Local\n          Offset: 1\n          Scale: 1\n          Value: true\n        Position:\n          Alpha: 0.30000001192092896\n          Color: 204; 51; 204\n          Scale: 1\n          Value: true\n        Value: true\n      Enabled: true\n      Keep: 1\n      Name: Pose_world\n      Position Tolerance: 0.10000000149011612\n      Queue Size: 10\n      Shape:\n        Alpha: 1\n        Axes Length: 1\n        Axes Radius: 0.10000000149011612\n        Color: 255; 25; 0\n        Head Length: 0.30000001192092896\n        Head Radius: 0.10000000149011612\n        Shaft Length: 1\n        Shaft Radius: 0.05000000074505806\n        Value: Axes\n      Topic: /ingvio_estimator/pose_w\n      Unreliable: false\n      Value: true\n    - Alpha: 1\n      Buffer Length: 1\n      Class: rviz/Path\n      Color: 173; 127; 168\n      Enabled: true\n      Head Diameter: 0.30000001192092896\n      Head Length: 0.20000000298023224\n      Length: 0.30000001192092896\n      Line Style: Lines\n      Line Width: 0.029999999329447746\n      Name: Path_spp\n      Offset:\n        X: 0\n        Y: 0\n        Z: 0\n      Pose Color: 255; 85; 255\n      Pose Style: None\n      Queue Size: 1\n      Radius: 0.029999999329447746\n      Shaft Diameter: 0.10000000149011612\n      Shaft Length: 0.10000000149011612\n      Topic: /ingvio_estimator/path_spp\n      Unreliable: false\n      Value: true\n    - Alpha: 1\n      Buffer Length: 1\n      Class: rviz/Path\n      Color: 239; 41; 41\n      Enabled: true\n      Head Diameter: 0.30000001192092896\n      Head Length: 0.20000000298023224\n      Length: 0.30000001192092896\n      Line Style: Lines\n      Line Width: 0.029999999329447746\n      Name: Path_rtk_gt\n      Offset:\n        X: 0\n        Y: 0\n        Z: 0\n      Pose Color: 255; 85; 255\n      Pose Style: None\n      Queue Size: 1\n      Radius: 0.029999999329447746\n      Shaft Diameter: 0.10000000149011612\n      Shaft Length: 0.10000000149011612\n      Topic: /ingvio_estimator/path_gt\n      Unreliable: false\n      Value: true\n  Enabled: true\n  Global Options:\n    Background Color: 48; 48; 48\n    Default Light: true\n    Fixed Frame: world\n    Frame Rate: 30\n  Name: root\n  Tools:\n    - Class: rviz/Interact\n      Hide Inactive Objects: true\n    - Class: rviz/MoveCamera\n    - Class: rviz/Select\n    - Class: rviz/FocusCamera\n    - Class: rviz/Measure\n    - Class: rviz/SetInitialPose\n      Theta std deviation: 0.2617993950843811\n      Topic: /initialpose\n      X std deviation: 0.5\n      Y std deviation: 0.5\n    - Class: rviz/SetGoal\n      Topic: /move_base_simple/goal\n    - Class: rviz/PublishPoint\n      Single click: true\n      Topic: /clicked_point\n  Value: true\n  Views:\n    Current:\n      Class: rviz/Orbit\n      Distance: 24.759632110595703\n      Enable Stereo Rendering:\n        Stereo Eye Separation: 0.05999999865889549\n        Stereo Focal Distance: 1\n        Swap Stereo Eyes: false\n        Value: false\n      Field of View: 0.7853981852531433\n      Focal Point:\n        X: 0\n        Y: 0\n        Z: 0\n      Focal Shape Fixed Size: true\n      Focal Shape Size: 0.05000000074505806\n      Invert Z Axis: false\n      Name: Current View\n      Near Clip Distance: 0.009999999776482582\n      Pitch: 0.4253981113433838\n      Target Frame: <Fixed Frame>\n      Yaw: 0.6503981351852417\n    Saved: ~\nWindow Geometry:\n  Displays:\n    collapsed: false\n  Height: 1014\n  Hide Left Dock: false\n  Hide Right Dock: false\n  MonoTrack:\n    collapsed: false\n  QMainWindow State: 000000ff00000000fd00000004000000000000025b00000358fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000016a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000012004d006f006e006f0054007200610063006b01000001ad000001e80000001600ffffff000000010000010f00000358fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000358000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005ac0000003efc0100000002fb0000000800540069006d00650100000000000005ac000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000002360000035800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000\n  Selection:\n    collapsed: false\n  Time:\n    collapsed: false\n  Tool Properties:\n    collapsed: false\n  Views:\n    collapsed: false\n  Width: 1452\n  X: 246\n  Y: 27\n"
  },
  {
    "path": "ingvio_estimator/rviz/ingvio_stereo.rviz",
    "content": "Panels:\n  - Class: rviz/Displays\n    Help Height: 78\n    Name: Displays\n    Property Tree Widget:\n      Expanded:\n        - /Global Options1\n        - /Status1\n      Splitter Ratio: 0.5\n    Tree Height: 268\n  - Class: rviz/Selection\n    Name: Selection\n  - Class: rviz/Tool Properties\n    Expanded:\n      - /2D Pose Estimate1\n      - /2D Nav Goal1\n      - /Publish Point1\n    Name: Tool Properties\n    Splitter Ratio: 0.5886790156364441\n  - Class: rviz/Views\n    Expanded:\n      - /Current View1\n    Name: Views\n    Splitter Ratio: 0.5\n  - Class: rviz/Time\n    Name: Time\n    SyncMode: 0\n    SyncSource: \"\"\nPreferences:\n  PromptSaveOnExit: true\nToolbars:\n  toolButtonStyle: 2\nVisualization Manager:\n  Class: \"\"\n  Displays:\n    - Alpha: 0.5\n      Cell Size: 1\n      Class: rviz/Grid\n      Color: 160; 160; 164\n      Enabled: true\n      Line Style:\n        Line Width: 0.029999999329447746\n        Value: Lines\n      Name: Grid\n      Normal Cell Count: 0\n      Offset:\n        X: 0\n        Y: 0\n        Z: 0\n      Plane: XY\n      Plane Cell Count: 10\n      Reference Frame: <Fixed Frame>\n      Value: true\n    - Class: rviz/Image\n      Enabled: true\n      Image Topic: /stereo_tracker/stereo_track\n      Max Value: 1\n      Median window: 5\n      Min Value: 0\n      Name: StereoTrack\n      Normalize Range: true\n      Queue Size: 2\n      Transport Hint: raw\n      Unreliable: false\n      Value: true\n    - Alpha: 1\n      Buffer Length: 1\n      Class: rviz/Path\n      Color: 25; 255; 0\n      Enabled: true\n      Head Diameter: 0.30000001192092896\n      Head Length: 0.20000000298023224\n      Length: 0.30000001192092896\n      Line Style: Lines\n      Line Width: 0.029999999329447746\n      Name: Path_world\n      Offset:\n        X: 0\n        Y: 0\n        Z: 0\n      Pose Color: 255; 85; 255\n      Pose Style: None\n      Queue Size: 2\n      Radius: 0.029999999329447746\n      Shaft Diameter: 0.10000000149011612\n      Shaft Length: 0.10000000149011612\n      Topic: /ingvio_estimator/path_w\n      Unreliable: false\n      Value: true\n    - Angle Tolerance: 0.10000000149011612\n      Class: rviz/Odometry\n      Covariance:\n        Orientation:\n          Alpha: 0.5\n          Color: 255; 255; 127\n          Color Style: Unique\n          Frame: Local\n          Offset: 1\n          Scale: 1\n          Value: true\n        Position:\n          Alpha: 0.30000001192092896\n          Color: 204; 51; 204\n          Scale: 1\n          Value: true\n        Value: true\n      Enabled: true\n      Keep: 1\n      Name: Pose_world\n      Position Tolerance: 0.10000000149011612\n      Queue Size: 10\n      Shape:\n        Alpha: 1\n        Axes Length: 1\n        Axes Radius: 0.10000000149011612\n        Color: 255; 25; 0\n        Head Length: 0.30000001192092896\n        Head Radius: 0.10000000149011612\n        Shaft Length: 1\n        Shaft Radius: 0.05000000074505806\n        Value: Axes\n      Topic: /ingvio_estimator/pose_w\n      Unreliable: false\n      Value: true\n    - Alpha: 1\n      Buffer Length: 1\n      Class: rviz/Path\n      Color: 239; 41; 41\n      Enabled: true\n      Head Diameter: 0.30000001192092896\n      Head Length: 0.20000000298023224\n      Length: 0.30000001192092896\n      Line Style: Lines\n      Line Width: 0.029999999329447746\n      Name: Path_rtk_gt\n      Offset:\n        X: 0\n        Y: 0\n        Z: 0\n      Pose Color: 255; 85; 255\n      Pose Style: None\n      Queue Size: 1\n      Radius: 0.029999999329447746\n      Shaft Diameter: 0.10000000149011612\n      Shaft Length: 0.10000000149011612\n      Topic: /ingvio_estimator/path_gt\n      Unreliable: false\n      Value: true\n    - Alpha: 1\n      Buffer Length: 1\n      Class: rviz/Path\n      Color: 173; 127; 168\n      Enabled: true\n      Head Diameter: 0.30000001192092896\n      Head Length: 0.20000000298023224\n      Length: 0.30000001192092896\n      Line Style: Lines\n      Line Width: 0.029999999329447746\n      Name: Path_spp\n      Offset:\n        X: 0\n        Y: 0\n        Z: 0\n      Pose Color: 255; 85; 255\n      Pose Style: None\n      Queue Size: 1\n      Radius: 0.029999999329447746\n      Shaft Diameter: 0.10000000149011612\n      Shaft Length: 0.10000000149011612\n      Topic: /ingvio_estimator/path_spp\n      Unreliable: false\n      Value: true\n  Enabled: true\n  Global Options:\n    Background Color: 48; 48; 48\n    Default Light: true\n    Fixed Frame: world\n    Frame Rate: 30\n  Name: root\n  Tools:\n    - Class: rviz/Interact\n      Hide Inactive Objects: true\n    - Class: rviz/MoveCamera\n    - Class: rviz/Select\n    - Class: rviz/FocusCamera\n    - Class: rviz/Measure\n    - Class: rviz/SetInitialPose\n      Theta std deviation: 0.2617993950843811\n      Topic: /initialpose\n      X std deviation: 0.5\n      Y std deviation: 0.5\n    - Class: rviz/SetGoal\n      Topic: /move_base_simple/goal\n    - Class: rviz/PublishPoint\n      Single click: true\n      Topic: /clicked_point\n  Value: true\n  Views:\n    Current:\n      Class: rviz/Orbit\n      Distance: 24.759632110595703\n      Enable Stereo Rendering:\n        Stereo Eye Separation: 0.05999999865889549\n        Stereo Focal Distance: 1\n        Swap Stereo Eyes: false\n        Value: false\n      Field of View: 0.7853981852531433\n      Focal Point:\n        X: 0\n        Y: 0\n        Z: 0\n      Focal Shape Fixed Size: true\n      Focal Shape Size: 0.05000000074505806\n      Invert Z Axis: false\n      Name: Current View\n      Near Clip Distance: 0.009999999776482582\n      Pitch: 0.6003981828689575\n      Target Frame: <Fixed Frame>\n      Yaw: 0.84539794921875\n    Saved: ~\nWindow Geometry:\n  Displays:\n    collapsed: false\n  Height: 875\n  Hide Left Dock: false\n  Hide Right Dock: false\n  QMainWindow State: 000000ff00000000fd0000000400000000000001e8000002cdfc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000197000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001600530074006500720065006f0054007200610063006b01000001da000001300000001600ffffff000000010000010f000002cdfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002cd000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005930000003efc0100000002fb0000000800540069006d0065010000000000000593000003bc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000290000002cd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000\n  Selection:\n    collapsed: false\n  StereoTrack:\n    collapsed: false\n  Time:\n    collapsed: false\n  Tool Properties:\n    collapsed: false\n  Views:\n    collapsed: false\n  Width: 1427\n  X: 209\n  Y: 54\n"
  },
  {
    "path": "ingvio_estimator/src/AnchoredLandmark.cpp",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n*    inertial-raw GNSS navigation. \n*    \n*    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n*                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n*    \n*    This program is free software: you can redistribute it and/or modify\n*    it under the terms of the GNU General Public License as published by\n*    the Free Software Foundation, either version 3 of the License, or\n*    (at your option) any later version.\n*    \n*    This program is distributed in the hope that it will be useful,\n*    but WITHOUT ANY WARRANTY; without even the implied warranty of\n*    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n*    GNU General Public License for more details.\n*    \n*    You should have received a copy of the GNU General Public License\n*    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n*/\n\n#include \"AnchoredLandmark.h\"\n\nnamespace ingvio\n{\n    void AnchoredLandmark::switchRepXyz2Invd(const Eigen::Vector3d& xyz, Eigen::Vector3d& invd)\n    {\n        assert(xyz.z() != 0.0);\n        if (xyz.z() < 0)\n            std::cout << \"[AnchoredLandmark]: Warning xyz.z() < 0 !\" << std::endl;\n        \n        invd.z() = 1.0/xyz.z();\n        invd.x() = xyz.x()*invd.z();\n        invd.y() = xyz.y()*invd.z();\n    }\n    \n    void AnchoredLandmark::switchRepInvd2Xyz(const Eigen::Vector3d& invd, Eigen::Vector3d& xyz)\n    {\n        assert(invd.z() != 0.0);\n        if (invd.z() < 0)\n            std::cout << \"[AnchoredLandmark]: Warning invd.z() < 0 !\" << std::endl;\n        \n        xyz.z() = 1.0/invd.z();\n        xyz.x() = invd.x()*xyz.z();\n        xyz.y() = invd.y()*xyz.z();\n    }\n    \n    void AnchoredLandmark::switchRepXyz2Bear(const Eigen::Vector3d& xyz, Eigen::Vector3d& bear)\n    {\n        double r = xyz.norm();\n        \n        assert(r != 0.0);\n        double rho = 1.0/r;\n        bear.z() = rho;\n        bear.x() = std::atan2(xyz.y(), xyz.x());\n        bear.y() = std::acos(rho*xyz.z());\n    }\n    \n    void AnchoredLandmark::switchRepBear2Xyz(const Eigen::Vector3d& bear, Eigen::Vector3d& xyz)\n    {\n        assert(bear.z() != 0.0);\n        if (bear.z() < 0)\n            std::cout << \"[AnchoredLandmark]: Warning bear.z() < 0 !\" << std::endl;\n        \n        double r = 1.0/bear.z();\n        xyz.z() = r*std::cos(bear.y());\n        xyz.x() = r*std::cos(bear.x())*std::sin(bear.y());\n        xyz.y() = r*std::sin(bear.x())*std::sin(bear.y());\n    }\n    \n    void AnchoredLandmark::setRepType(const AnchoredLandmark::LmRep& other_rep_type)\n    {\n        if (!_has_rep_set)\n        {\n            _landmark_param_type = other_rep_type;\n            _has_rep_set = true;\n        }\n        else\n            std::cout << \"[AnchoredLandmark]: Representation type has been set once! Cannot change!\" << std::endl;\n    }\n    \n    void AnchoredLandmark::resetAnchoredPose(std::shared_ptr<SE3> new_anchored_pose,\n                                             bool isUpdateRepValue)\n    {\n        if (new_anchored_pose != nullptr)\n        {\n            _anchored_pose = new_anchored_pose;\n            \n            if (isUpdateRepValue)\n            {\n                this->setValuePosXyz(this->_pos_xyz);\n                \n                // TODO: The anchored pose may not take fej?\n                this->setFejPosXyz(this->_pos_xyz_fej);\n            }\n        }\n        else\n            _anchored_pose.reset();\n    }\n    \n    void AnchoredLandmark::setValuePosXyz(const Eigen::Vector3d& xyz_world)\n    {\n        _pos_xyz = xyz_world;\n        \n        if (_anchored_pose != nullptr)\n        {\n            Eigen::Vector3d xyz_body = _anchored_pose->valueLinearAsMat().transpose()*(_pos_xyz - _anchored_pose->valueTrans());\n            \n            switch (_landmark_param_type)\n            {\n                case XYZ:\n                    _pos_rep = xyz_body;\n                    break;\n                case INV_DEPTH:\n                    switchRepXyz2Invd(xyz_body, _pos_rep);\n                    break;\n                case BEARING:\n                    switchRepXyz2Bear(xyz_body, _pos_rep);\n                    break;\n                default:\n                    assert(false);\n                    break;\n            }\n        }\n        else\n            std::cout << \"[AnchoredLandmark]: Cannot set body rep, no anchored pose!\" << std::endl;\n    }\n    \n    void AnchoredLandmark::setValuePosRep(const Eigen::Vector3d& pos_rep_body)\n    {\n        _pos_rep = pos_rep_body;\n        \n        Eigen::Vector3d xyz_body;\n        \n        switch (_landmark_param_type)\n        {\n            case XYZ:\n                xyz_body = _pos_rep;\n                break;\n            case INV_DEPTH:\n                switchRepInvd2Xyz(_pos_rep, xyz_body);\n                break;\n            case BEARING:\n                switchRepBear2Xyz(_pos_rep, xyz_body);\n                break;\n            default:\n                assert(false);\n                break;\n        }\n        \n        if (_anchored_pose != nullptr)\n            _pos_xyz = _anchored_pose->valueLinearAsMat()*xyz_body + _anchored_pose->valueTrans();\n        else\n            std::cout << \"[AnchoredLandmark]: Cannot set world xyz pos, no anchored pose!\" << std::endl;\n    }\n    \n    void AnchoredLandmark::setFejPosXyz(const Eigen::Vector3d& xyz_world_fej)\n    {\n        _pos_xyz_fej = xyz_world_fej;\n        \n        if (_anchored_pose != nullptr)\n        {\n            Eigen::Vector3d xyz_body_fej = _anchored_pose->fejLinearAsMat().transpose()*(_pos_xyz_fej - _anchored_pose->fejTrans());\n            \n            switch (_landmark_param_type)\n            {\n                case XYZ:\n                    _pos_rep_fej = xyz_body_fej;\n                    break;\n                case INV_DEPTH:\n                    switchRepXyz2Invd(xyz_body_fej, _pos_rep_fej);\n                    break;\n                case BEARING:\n                    switchRepXyz2Bear(xyz_body_fej, _pos_rep_fej);\n                    break;\n                default:\n                    assert(false);\n                    break;\n            }\n        }\n        else\n            std::cout << \"[AnchoredLandmark]: Cannot set body rep fej, no anchored pose!\" << std::endl;\n    }\n    \n    void AnchoredLandmark::setFejPosRep(const Eigen::Vector3d& pos_rep_body_fej)\n    {\n        _pos_rep_fej = pos_rep_body_fej;\n        \n        Eigen::Vector3d xyz_body_fej;\n        \n        switch (_landmark_param_type)\n        {\n            case XYZ:\n                xyz_body_fej = _pos_rep_fej;\n                break;\n            case INV_DEPTH:\n                switchRepInvd2Xyz(_pos_rep_fej, xyz_body_fej);\n                break;\n            case BEARING:\n                switchRepBear2Xyz(_pos_rep_fej, xyz_body_fej);\n                break;\n            default:\n                assert(false);\n                break;\n        }\n        \n        if (_anchored_pose != nullptr)\n            _pos_xyz_fej = _anchored_pose->fejLinearAsMat()*xyz_body_fej + _anchored_pose->fejTrans();\n        else\n            std::cout << \"[AnchoredLandmark]: Cannot set world xyz pos fej, no anchored pose!\" << std::endl;\n    }\n    \n    std::shared_ptr<AnchoredLandmark> AnchoredLandmark::clone()\n    {\n        std::shared_ptr<AnchoredLandmark> tmp(new AnchoredLandmark());\n        \n        tmp->setRepType(this->checkLmRepType());\n        \n        tmp->resetAnchoredPose(this->getAnchoredPose());\n        \n        tmp->setValuePosXyz(this->valuePosXyz());\n        \n        tmp->setFejPosXyz(this->fejPosXyz());\n        \n        return tmp;\n    }\n    \n    void AnchoredLandmark::update(const Eigen::VectorXd& dx)\n    {\n        assert(dx.rows() >= this->idx() + this->size());\n        const Eigen::Vector3d& delta_p = dx.block<3, 1>(this->idx(), 0);\n        \n        if (_anchored_pose != nullptr)\n        {\n            assert(_anchored_pose->idx() + _anchored_pose->size() <= dx.rows());\n            const Eigen::Vector3d& delta_theta = dx.block<3, 1>(_anchored_pose->idx(), 0);\n            this->setValuePosXyz(GammaFunc(delta_theta, 0)*this->valuePosXyz() + GammaFunc(delta_theta, 1)*delta_p);\n        }\n        else\n        {\n            std::cout << \"[AnchoredLandmark]: Warning! Update without anchored pose!\" << std::endl;\n            this->setValuePosXyz(this->valuePosXyz() + delta_p);\n        }\n    }\n    \n    void AnchoredLandmark::setIdentity()\n    {\n        _pos_rep.setZero();\n        _pos_rep_fej.setZero();\n        \n        _pos_xyz.setZero();\n        _pos_xyz_fej.setZero();\n    }\n    \n    void AnchoredLandmark::setRandom()\n    {\n        setValuePosXyz(Eigen::Vector3d::Random());\n    }\n}\n"
  },
  {
    "path": "ingvio_estimator/src/AnchoredLandmark.h",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#pragma once\n\n#include <iostream>\n#include \"PoseState.h\"\n\nnamespace ingvio\n{\n    class AnchoredLandmark : public Type\n    {\n    public:\n        enum LmRep {XYZ, INV_DEPTH, BEARING};\n        \n        AnchoredLandmark() : Type(3)\n        {\n            _pos_rep.setZero();\n            _pos_rep_fej.setZero();\n            \n            _pos_xyz.setZero();\n            _pos_xyz_fej.setZero();\n            \n            _landmark_param_type = INV_DEPTH;\n            _has_rep_set = false;\n            \n            _anchored_pose.reset();\n        }\n        \n        ~AnchoredLandmark() {}\n        \n        void update(const Eigen::VectorXd& dx) override;\n        \n        void setIdentity() override;\n        \n        void setRandom() override;\n        \n        void setRepType(const AnchoredLandmark::LmRep& other_rep_type);\n        \n        static void switchRepXyz2Invd(const Eigen::Vector3d& xyz, Eigen::Vector3d& invd);\n        \n        static void switchRepInvd2Xyz(const Eigen::Vector3d& invd, Eigen::Vector3d& xyz);\n        \n        static void switchRepXyz2Bear(const Eigen::Vector3d& xyz, Eigen::Vector3d& bear);\n        \n        static void switchRepBear2Xyz(const Eigen::Vector3d& bear, Eigen::Vector3d& xyz);\n        \n        void resetAnchoredPose(std::shared_ptr<SE3> new_anchored_pose = nullptr,\n                               bool isUpdateRepValue = false);\n        \n        const std::shared_ptr<SE3> getAnchoredPose() const\n        { return _anchored_pose; }\n        \n        const Eigen::Vector3d& valuePosRep() const\n        { return _pos_rep; }\n        \n        const Eigen::Vector3d& valuePosXyz() const\n        { return _pos_xyz; }\n        \n        const Eigen::Vector3d& fejPosRep() const\n        { return _pos_rep_fej; }\n        \n        const Eigen::Vector3d& fejPosXyz() const\n        { return _pos_xyz_fej; }\n        \n        const LmRep& checkLmRepType() const\n        { return _landmark_param_type; }\n        \n        void setValuePosXyz(const Eigen::Vector3d& xyz_world);\n        \n        void setValuePosRep(const Eigen::Vector3d& pos_rep_body);\n        \n        void setFejPosXyz(const Eigen::Vector3d& xyz_world_fej);\n        \n        void setFejPosRep(const Eigen::Vector3d& pos_rep_body_fej);\n        \n        std::shared_ptr<AnchoredLandmark> clone();\n        \n    protected:\n        LmRep _landmark_param_type;\n        \n        Eigen::Vector3d _pos_rep;\n        Eigen::Vector3d _pos_rep_fej;\n        \n        Eigen::Vector3d _pos_xyz;\n        Eigen::Vector3d _pos_xyz_fej;\n        \n        std::shared_ptr<SE3> _anchored_pose;\n        \n        bool _has_rep_set;\n    };\n}\n\n"
  },
  {
    "path": "ingvio_estimator/src/AuxGammaFunc.cpp",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#include <assert.h>\n#include <cmath>\n\n#include \"AuxGammaFunc.h\"\n\nnamespace ingvio\n{\n    Eigen::Matrix3d skew(const Eigen::Vector3d& vec)\n    {\n        Eigen::Matrix3d result;\n        result << 0.0, -vec.z(),  vec.y(),\n        vec.z(),      0.0, -vec.x(),\n        -vec.y(),  vec.x(),      0.0;\n        return result;\n    }\n    \n    Eigen::Vector3d vee(const Eigen::Matrix3d& mat)\n    {\n        Eigen::Vector3d result;\n        result.z() = mat(1, 0);\n        result.y() = mat(0, 2);\n        result.x() = mat(2, 1);\n        return result;\n    }\n    \n    Eigen::Matrix3d GammaFunc(const Eigen::Vector3d& vec, int m)\n    {\n        assert(m >= 0 && m <= 3);\n        \n        Eigen::Matrix3d result;\n        \n        double theta = vec.norm();\n        if (std::fabs(theta) < 1e-06)\n        {\n            double factor = 1.0;\n            switch (m)\n            {\n                case 3:\n                    factor = 1.0/6.0;\n                    break;\n                case 2:\n                    factor = 0.5;\n                    break;\n                default:\n                    break;\n            }\n            return factor*Eigen::Matrix3d::Identity();\n        }\n        \n        Eigen::Vector3d n = vec.normalized();\n        Eigen::Matrix3d n_cross = skew(n);\n        Eigen::Matrix3d n_cross2 = n_cross*n_cross;\n        \n        double factor0, factor1, factor2;\n        double sintheta = std::sin(theta);\n        double costheta = std::cos(theta);\n        \n        switch (m)\n        {\n            case 1:\n            {\n                factor0 = 1.0;\n                factor1 = (1.0 - costheta)/theta;\n                factor2 = (theta - sintheta)/theta;\n                break;\n            }\n            case 2:\n            {\n                factor0 = 0.5;\n                factor1 = (theta-sintheta)/std::pow(theta, 2);\n                factor2 = (std::pow(theta, 2)+2.0*costheta-2.0)/(2.0*std::pow(theta, 2));\n                break;\n            }\n            case 3:\n            {\n                factor0 = 1.0/6.0;\n                double theta3 = std::pow(theta, 3);\n                factor1 = (std::pow(theta, 2)+2.0*costheta-2.0)/(2.0*theta3);\n                factor2 = (theta3-6.0*theta+6.0*sintheta)/(6.0*theta3);\n                break;\n            }\n            default:\n            {\n                factor0 = 1.0;\n                factor1 = sintheta;\n                factor2 = 1.0 - costheta;\n                break;\n            }\n        }\n        \n        result = factor0*Eigen::Matrix3d::Identity() + factor1*n_cross + factor2*n_cross2;\n        return result;\n    }\n    \n    Eigen::Matrix3d Psi1Func(const Eigen::Vector3d& tilde_omega,\n                             const Eigen::Vector3d& tilde_acc,\n                             double dt)\n    {\n        if ((tilde_omega*dt).norm() < 1e-08) return Eigen::Matrix3d::Zero();\n        \n        Eigen::Matrix3d M1 = skew(tilde_acc)*GammaFunc(-tilde_omega*dt, 2)*std::pow(dt, 2.0);\n        \n        Eigen::Matrix3d WA = skew(tilde_omega)*skew(tilde_acc);\n        \n        Eigen::Matrix3d WAW = WA*skew(tilde_omega);\n        \n        Eigen::Matrix3d WAW2 = WAW*skew(tilde_omega);\n        \n        Eigen::Matrix3d W2A = skew(tilde_omega)*WA;\n        \n        Eigen::Matrix3d W2AW = W2A*skew(tilde_omega);\n        \n        Eigen::Matrix3d W2AW2 = W2AW*skew(tilde_omega);\n        \n        double eta = tilde_omega.norm();\n        \n        double xi = eta*dt;\n        double xi2 = std::pow(xi, 2.0);\n        \n        double sin_xi = std::sin(xi);\n        double cos_xi = std::cos(xi);\n        \n        double sin_2xi = std::sin(2*xi);\n        double cos_2xi = std::cos(2*xi);\n        \n        double eta3 = std::pow(eta, 3);\n        double eta4 = eta*eta3;\n        double eta5 = eta*eta4;\n        double eta6 = eta*eta5;\n        \n        double c1 = (sin_xi-xi*cos_xi)/eta3;\n        \n        double c2 = (cos_2xi-4*cos_xi+3)/(4*eta4);\n        \n        double c3 = (4*sin_xi+sin_2xi-4*xi*cos_xi-2*xi)/(4*eta5);\n        \n        double c4 = (xi2-2*xi*sin_xi-2*cos_xi+2)/(2*eta4);\n        \n        double c5 = (6*xi-8*sin_xi+sin_2xi)/(4*eta5);\n        \n        double c6 = (2*xi2-4*xi*sin_xi-cos_2xi+1)/(4*eta6);\n        \n        Eigen::Matrix3d result = M1*(c1*WA + c2*WAW + c3*WAW2 + c4*W2A + c5*W2AW + c6*W2AW2);\n        \n        return result;\n    }\n    \n    Eigen::Matrix3d Psi2Func(const Eigen::Vector3d& tilde_omega,\n                             const Eigen::Vector3d& tilde_acc,\n                             double dt)\n    {\n        if ((tilde_omega*dt).norm() < 1e-07)\n            return Eigen::Matrix3d::Zero();\n        \n        Eigen::Matrix3d M1 = skew(tilde_acc)*GammaFunc(-tilde_omega*dt, 3)*std::pow(dt, 3);\n        \n        Eigen::Matrix3d WA = skew(tilde_omega)*skew(tilde_acc);\n        \n        Eigen::Matrix3d WAW = WA*skew(tilde_omega);\n        \n        Eigen::Matrix3d WAW2 = WAW*skew(tilde_omega);\n        \n        Eigen::Matrix3d W2A = skew(tilde_omega)*WA;\n        \n        Eigen::Matrix3d W2AW = W2A*skew(tilde_omega);\n        \n        Eigen::Matrix3d W2AW2 = W2AW*skew(tilde_omega);\n        \n        double eta = tilde_omega.norm();\n        \n        double xi = eta*dt;\n        \n        double xi2 = std::pow(xi, 2.0);\n        double xi3 = xi*xi2;\n        \n        double sin_xi = std::sin(xi);\n        double cos_xi = std::cos(xi);\n        \n        double sin_2xi = std::sin(2*xi);\n        double cos_2xi = std::cos(2*xi);\n        \n        double eta3 = std::pow(eta, 3);\n        double eta4 = eta*eta3;\n        double eta5 = eta*eta4;\n        double eta6 = eta*eta5;\n        double eta7 = eta*eta6;\n        \n        double c1 = (xi*sin_xi+2*cos_xi-2)/eta4;\n        \n        double c2 = (6*xi-8*sin_xi+sin_2xi)/(8*eta5);\n        \n        double c3 = (2*xi2+8*xi*sin_xi+16*cos_xi+cos_2xi-17)/(8*eta6);\n        \n        double c4 = (xi3+6*xi-12*sin_xi+6*xi*cos_xi)/(6*eta5);\n        \n        double c5 = (6*xi2+16*cos_xi-cos_2xi-15)/(8*eta6);\n        \n        double c6 = (4*xi3+6*xi-24*sin_xi-3*sin_2xi+24*xi*cos_xi)/(24*eta7);\n        \n        Eigen::Matrix3d result;\n        \n        result = M1*(c1*WA + c2*WAW + c3*WAW2 + c4*W2A + c5*W2AW + c6*W2AW2);\n        \n        return result;\n    }\n}\n"
  },
  {
    "path": "ingvio_estimator/src/AuxGammaFunc.h",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#pragma once\n\n#include <Eigen/Core>\n\nnamespace ingvio\n{\n    extern Eigen::Matrix3d skew(const Eigen::Vector3d& vec);\n    \n    extern Eigen::Vector3d vee(const Eigen::Matrix3d& mat);\n    \n    extern Eigen::Matrix3d GammaFunc(const Eigen::Vector3d& vec, int m = 0);\n    \n    extern Eigen::Matrix3d Psi1Func(const Eigen::Vector3d& tilde_omega,\n                                    const Eigen::Vector3d& tilde_acc,\n                                    double dt);\n    \n    extern Eigen::Matrix3d Psi2Func(const Eigen::Vector3d& tilde_omega,\n                                    const Eigen::Vector3d& tilde_acc,\n                                    double dt);\n}\n"
  },
  {
    "path": "ingvio_estimator/src/Color.h",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#pragma once\n\n#include <iostream>\n\nnamespace color\n{\n    inline std::ostream& setBlue(std::ostream& s)\n    {\n        s << \"\\033[0;1;34m\";\n        return s;\n    }\n    \n    inline std::ostream& setRed(std::ostream& s)\n    {\n        s << \"\\033[0;1;31m\";\n        return s;\n    }\n    \n    inline std::ostream& setGreen(std::ostream& s)\n    {\n        s << \"\\033[0;1;32m\";\n        return s;\n    }\n    \n    inline std::ostream& setYellow(std::ostream& s)\n    {\n        s << \"\\033[0;1;33m\";\n        return s;\n    }\n    \n    inline std::ostream& setWhite(std::ostream& s)\n    {\n        s << \"\\033[0;1;37m\";\n        return s;\n    }\n    \n    inline std::ostream& resetColor(std::ostream& s)\n    {\n        s << \"\\033[0m\";\n        return s;\n    }\n}\n"
  },
  {
    "path": "ingvio_estimator/src/GnssData.cpp",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#include \"GnssData.h\"\n\nnamespace ingvio\n{\n    void GnssData::inputEphem(gnss_comm::EphemBasePtr ephem_ptr)\n    {\n        double toe = gnss_comm::time2sec(ephem_ptr->toe);\n        \n        if (this->sat2time_index.count(ephem_ptr->sat) == 0 || \n            this->sat2time_index.at(ephem_ptr->sat).count(toe) == 0)\n        {\n            this->sat2ephem[ephem_ptr->sat].emplace_back(ephem_ptr);\n            this->sat2time_index[ephem_ptr->sat].emplace(toe, sat2ephem.at(ephem_ptr->sat).size()-1);\n        }\n    }\n}\n"
  },
  {
    "path": "ingvio_estimator/src/GnssData.h",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#pragma once\n\n#include <gnss_comm/gnss_ros.hpp>\n\nnamespace ingvio\n{\n    class GnssData\n    {\n    public:\n        \n        GnssData() : _isIono(false)\n        {}\n        \n        ~GnssData() = default;\n        \n        GnssData operator=(const GnssData&) = delete;\n        \n        std::vector<double> latest_gnss_iono_params;\n        \n        std::map<uint32_t, std::vector<gnss_comm::EphemBasePtr>> sat2ephem;\n        \n        std::map<uint32_t, std::map<double, size_t>> sat2time_index;\n        \n        std::map<uint32_t, uint32_t> sat_track_status;\n        \n        void inputEphem(gnss_comm::EphemBasePtr ephem_ptr);\n        \n        bool _isIono;\n        \n    private:\n        \n        GnssData(const GnssData&) {}\n    };\n}\n"
  },
  {
    "path": "ingvio_estimator/src/GnssManager.cpp",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#include \"GnssManager.h\"\n\nnamespace ingvio\n{\n    const std::unordered_map<uint32_t, State::GNSSType> GnssManager::satsys_to_gnsstype = \n    {\n        {SYS_GPS, State::GNSSType::GPS},\n        {SYS_BDS, State::GNSSType::BDS},\n        {SYS_GAL, State::GNSSType::GAL},\n        {SYS_GLO, State::GNSSType::GLO}\n    };\n    \n    const std::unordered_map<State::GNSSType, uint32_t> GnssManager::gnsstype_to_satsys = \n    {\n        {State::GNSSType::GPS, SYS_GPS},\n        {State::GNSSType::BDS, SYS_BDS},\n        {State::GNSSType::GAL, SYS_GAL},\n        {State::GNSSType::GLO, SYS_GLO}\n    };\n    \n    const std::unordered_map<int, State::GNSSType> GnssManager::idx_to_gnsstype =\n    {\n        {0, State::GNSSType::GPS},\n        {1, State::GNSSType::GLO},\n        {2, State::GNSSType::GAL},\n        {3, State::GNSSType::BDS}\n    };\n    \n    State::GNSSType GnssManager::convertSatsys2GnssType(const uint32_t& sat_sys)\n    {\n        if (GnssManager::satsys_to_gnsstype.find(sat_sys) == GnssManager::satsys_to_gnsstype.end())\n        {\n            std::cout << \"[GnssManager]: Sat sys not in index map, cannot convert to gnss type!\" << std::endl;\n            std::exit(EXIT_FAILURE);\n        }\n        \n        return GnssManager::satsys_to_gnsstype.at(sat_sys);\n    }\n    \n    Eigen::Matrix3d GnssManager::calcRw2enu(const double& yaw_offset)\n    {\n        return Eigen::AngleAxisd(yaw_offset, Eigen::Vector3d::UnitZ()).toRotationMatrix();\n    }\n    \n    Eigen::Isometry3d GnssManager::calcTw2enu(const double& yaw_offset)\n    {\n        Eigen::Isometry3d result = Eigen::Isometry3d::Identity();\n        result.linear() = GnssManager::calcRw2enu(yaw_offset);\n        return result;\n    }\n    \n    Eigen::Vector4d GnssManager::getClockbiasVec(const std::shared_ptr<State> state)\n    {\n        Eigen::Vector4d cb = Eigen::Vector4d::Zero();\n        \n        for (const auto& gnss_item : state->_gnss)\n            if (gnss_item.first == State::YOF || gnss_item.first == State::FS)\n                continue;\n            else\n                cb(gnss_comm::sys2idx.at(GnssManager::gnsstype_to_satsys.at(gnss_item.first)))\n                = gnss_item.second->value();\n            \n        return cb;\n    }\n    \n    bool GnssManager::checkGnssStates(const std::shared_ptr<State> state)\n    {\n        if (state->_gnss.find(State::GNSSType::YOF) == state->_gnss.end())\n            return false;\n        \n        if (state->_gnss.find(State::GNSSType::FS) == state->_gnss.end())\n            return false;\n        \n        for (int i = 0; i < 4; ++i)\n            if (state->_gnss.find(static_cast<State::GNSSType>(i)) != state->_gnss.end())\n                return true;\n            \n        return false;\n    }\n    \n    Eigen::Matrix3d GnssManager::dotRw2enu(const std::shared_ptr<State> state)\n    {\n        if (state->_gnss.find(State::GNSSType::YOF) == state->_gnss.end())\n            return Eigen::Matrix3d::Zero();\n        \n        const double yo = state->_gnss.at(State::GNSSType::YOF)->value();\n        \n        Eigen::Matrix3d result;\n        result << -std::sin(yo), -std::cos(yo), 0.0,\n                   std::cos(yo), -std::sin(yo), 0.0,\n                            0.0,           0.0, 0.0;\n                            \n        return result;\n    }\n    \n    Eigen::Matrix3d GnssManager::calcRw2enu(const std::shared_ptr<State> state)\n    {\n        if (state->_gnss.find(State::GNSSType::YOF) == state->_gnss.end())\n            return Eigen::Matrix3d::Identity();\n        \n        const double yo = state->_gnss.at(State::GNSSType::YOF)->value();\n        \n        return GnssManager::calcRw2enu(yo);\n    }\n    \n    Eigen::Isometry3d GnssManager::calcTw2enu(const std::shared_ptr<State> state)\n    {\n        if (state->_gnss.find(State::GNSSType::YOF) == state->_gnss.end())\n            return Eigen::Isometry3d::Identity();\n        \n        const double yo = state->_gnss.at(State::GNSSType::YOF)->value();\n        \n        return GnssManager::calcTw2enu(yo);\n    }\n    \n    void GnssManager::printYOF(const std::shared_ptr<State> state)\n    {\n        if (state->_gnss.find(State::GNSSType::YOF) == state->_gnss.end())\n            return;\n        \n        std::cout << \"[GnssManager]: Local to ENU yaw offset = \" << state->_gnss.at(State::GNSSType::YOF)->value()*180.0/M_PI << \" (deg)\" << std::endl;\n    }\n}\n"
  },
  {
    "path": "ingvio_estimator/src/GnssManager.h",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#pragma once\n\n#include <unordered_map>\n\n#include <gnss_comm/gnss_ros.hpp>\n\n#include \"State.h\"\n\nnamespace ingvio\n{\n    class GnssManager\n    {\n    public:\n        GnssManager() = delete;\n        \n        GnssManager(const GnssManager&) = delete;\n        GnssManager operator=(const GnssManager&) = delete;\n        \n        static const std::unordered_map<uint32_t, State::GNSSType> satsys_to_gnsstype;\n        \n        static const std::unordered_map<State::GNSSType, uint32_t> gnsstype_to_satsys;\n        \n        static const std::unordered_map<int, State::GNSSType> idx_to_gnsstype;\n        \n        static State::GNSSType convertSatsys2GnssType(const uint32_t& sat_sys);\n        \n        static Eigen::Matrix3d calcRw2enu(const double& yaw_offset);\n        \n        static Eigen::Isometry3d calcTw2enu(const double& yaw_offset);\n        \n        static Eigen::Matrix3d calcRw2enu(const std::shared_ptr<State> state);\n        \n        static Eigen::Isometry3d calcTw2enu(const std::shared_ptr<State> state);\n        \n        static Eigen::Vector4d getClockbiasVec(const std::shared_ptr<State> state);\n        \n        static bool checkGnssStates(const std::shared_ptr<State> state);\n        \n        static Eigen::Matrix3d dotRw2enu(const std::shared_ptr<State> state);\n        \n        static void printYOF(const std::shared_ptr<State> state);\n    };\n}\n"
  },
  {
    "path": "ingvio_estimator/src/GnssProcessor.cpp",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#include <gnss_comm/gnss_spp.hpp>\n\n#include \"IngvioFilter.h\"\n\n#include <nav_msgs/Odometry.h>\n#include <eigen_conversions/eigen_msg.h>\n\n#include \"GnssData.h\"\n#include \"GnssSync.h\"\n#include \"GvioAligner.h\"\n\nnamespace ingvio\n{\n    void IngvioFilter::callbackRtkGroundTruth(\n        const sensor_msgs::NavSatFix::ConstPtr& nav_sat_msg)\n    {\n        if (!_filter_params._enable_gnss || !_gnss_sync->isSync()) return;\n        \n        \n        if (_gvio_aligner->isAlign() &&\n            nav_sat_msg->status.status >= sensor_msgs::NavSatStatus::STATUS_FIX)\n        {\n            Eigen::Vector3d gt_geo, gt_ecef;\n            gt_geo.x() = nav_sat_msg->latitude;\n            gt_geo.y() = nav_sat_msg->longitude;\n            gt_geo.z() = nav_sat_msg->altitude;\n            \n            gt_ecef = gnss_comm::geo2ecef(gt_geo);\n            \n            Eigen::Vector3d gt_w = _gvio_aligner->getTecef2w()*gt_ecef;\n            \n            if (gt_w.hasNaN())\n                return;\n            \n            geometry_msgs::PoseStamped pose_gt;\n            pose_gt.header.stamp = ros::Time().fromSec(nav_sat_msg->header.stamp.toSec() + _gnss_sync->getUnsyncTime());\n            pose_gt.header.frame_id = \"world\";\n            pose_gt.pose.position.x = gt_w.x();\n            pose_gt.pose.position.y = gt_w.y();\n            pose_gt.pose.position.z = gt_w.z();\n            pose_gt.pose.orientation.x = 0.0;\n            pose_gt.pose.orientation.y = 0.0;\n            pose_gt.pose.orientation.z = 0.0;\n            pose_gt.pose.orientation.w = 0.0;\n            \n            _path_gt_msg.header.stamp = pose_gt.header.stamp;\n            _path_gt_msg.header.frame_id = \"world\";\n            _path_gt_msg.poses.push_back(pose_gt);\n            _path_gt_pub.publish(_path_gt_msg);\n            \n            Eigen::Isometry3d T_i2w = Eigen::Isometry3d::Identity();\n            T_i2w.translation() = gt_w;\n            \n            nav_msgs::Odometry odom_gt_msg;\n            odom_gt_msg.header.stamp = pose_gt.header.stamp;\n            odom_gt_msg.header.frame_id = \"world\";\n            odom_gt_msg.child_frame_id = \"gt\";\n            \n            tf::poseEigenToMsg(T_i2w, odom_gt_msg.pose.pose);\n            tf::vectorEigenToMsg(Eigen::Vector3d::Zero(), odom_gt_msg.twist.twist.linear);\n            \n            _odom_gt_pub.publish(odom_gt_msg);\n        }\n    }\n    \n    void IngvioFilter::callbackEphem(const gnss_comm::GnssEphemMsgConstPtr& ephem_msg)\n    {\n        if (!_filter_params._enable_gnss) return;\n        \n        gnss_comm::EphemPtr ephem = gnss_comm::msg2ephem(ephem_msg);\n        \n        _gnss_data->inputEphem(ephem);\n    }\n    \n    void IngvioFilter::callbackGloEphem(const gnss_comm::GnssGloEphemMsgConstPtr& glo_ephem_msg)\n    {\n        if (!_filter_params._enable_gnss) return;\n        \n        gnss_comm::GloEphemPtr glo_ephem = gnss_comm::msg2glo_ephem(glo_ephem_msg);\n        \n        _gnss_data->inputEphem(glo_ephem);\n    }\n    \n    void IngvioFilter::callbackIonoParams(const gnss_comm::StampedFloat64ArrayConstPtr& iono_msg)\n    {\n        if (!_filter_params._enable_gnss) return;\n        \n        std::vector<double> iono_params;\n        std::copy(iono_msg->data.begin(), iono_msg->data.end(), std::back_inserter(iono_params));\n        \n        if (iono_params.size() != 8) return;\n        \n        _gnss_data->latest_gnss_iono_params.clear();\n        _gnss_data->latest_gnss_iono_params.assign(iono_params.begin(), iono_params.end());\n        \n        if (!_gnss_data->_isIono) _gnss_data->_isIono = true;\n    }\n    \n    void IngvioFilter::callbackGnssMeas(const gnss_comm::GnssMeasMsgConstPtr& gnss_meas_msg)\n    {\n        double aux_time = ros::Time::now().toSec();\n        \n        if (!_filter_params._enable_gnss || !_gnss_data->_isIono)\n            return;\n        \n        std::vector<gnss_comm::ObsPtr> gnss_meas = gnss_comm::msg2meas(gnss_meas_msg);\n        \n        if (gnss_meas.size() <= 0) return;\n        \n        _gnss_sync->storeTimePair(aux_time, gnss_meas[0]->time);\n        \n        if (!_gnss_sync->isSync()) return;\n        \n        std::vector<gnss_comm::ObsPtr> valid_meas;\n        std::vector<gnss_comm::EphemBasePtr> valid_ephems;\n        \n        for (auto obs : gnss_meas)\n        {\n            uint32_t sys = gnss_comm::satsys(obs->sat, NULL);\n            \n            if (sys != SYS_GPS && sys != SYS_GLO && sys != SYS_GAL && sys != SYS_BDS)\n                continue;\n            \n            if (_gnss_data->sat2ephem.count(obs->sat) == 0)\n                continue;\n            \n            if (obs->freqs.empty()) continue;\n            \n            int freq_idx = -1;\n            gnss_comm::L1_freq(obs, &freq_idx);\n            \n            if (freq_idx < 0) continue;\n            \n            double obs_time = gnss_comm::time2sec(obs->time);\n            \n            std::map<double, size_t> time2index = _gnss_data->sat2time_index.at(obs->sat);\n            \n            double ephem_time = EPH_VALID_SECONDS;\n            size_t ephem_index = -1;\n            \n            for (auto ti : time2index)\n            {\n                if (std::fabs(ti.first - obs_time) < ephem_time)\n                {\n                    ephem_time = std::fabs(ti.first - obs_time);\n                    ephem_index = ti.second;\n                }\n            }\n            \n            if (ephem_time >= EPH_VALID_SECONDS || ephem_index == -1)\n                continue;\n            \n            const gnss_comm::EphemBasePtr& best_ephem = _gnss_data->sat2ephem.at(obs->sat).at(ephem_index);\n            \n            if (obs->psr_std[freq_idx] > _filter_params._gnss_psr_std_thres || \n                obs->psr_std[freq_idx] > _filter_params._gnss_dopp_std_thres)\n            {\n                _gnss_data->sat_track_status[obs->sat] = 0;\n                continue;\n            }\n            else\n            {\n                if (_gnss_data->sat_track_status.count(obs->sat) == 0)\n                    _gnss_data->sat_track_status[obs->sat] = 0;\n                \n                ++_gnss_data->sat_track_status.at(obs->sat);\n            }\n            \n            if (_gnss_data->sat_track_status[obs->sat] < _filter_params._gnss_track_num_thres)\n                continue;\n            \n            valid_meas.push_back(obs);\n            valid_ephems.push_back(best_ephem);\n        }\n        \n        if (valid_meas.size() <= 0) \n            return;\n        else\n            _gnss_sync->bufferGnssMeas(valid_meas, valid_ephems);\n        \n        if (valid_meas.size() >= 4)\n        {\n            Eigen::Matrix<double, 7, 1> pos_raw_spp = gnss_comm::psr_pos(valid_meas,\n                valid_ephems, _gnss_data->latest_gnss_iono_params);\n            \n            if (pos_raw_spp.hasNaN() ||\n                pos_raw_spp.block<3, 1>(0, 0).norm() < 1e-03)\n                return;\n            \n            Eigen::Vector4d vel_raw_spp;\n            Eigen::Vector3d pos_ecef = pos_raw_spp.block<3, 1>(0, 0);\n            \n            vel_raw_spp = gnss_comm::dopp_vel(valid_meas, valid_ephems, pos_ecef);\n            \n            if (vel_raw_spp.hasNaN())\n                return;\n            \n            _gnss_sync->bufferSppMeas(valid_meas[0]->time, pos_raw_spp, vel_raw_spp);\n        }\n    }\n    \n}\n"
  },
  {
    "path": "ingvio_estimator/src/GnssSync.cpp",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#include <iomanip>\n\n#include \"GnssSync.h\"\n\nnamespace ingvio\n{\n    void GnssSync::bufferGnssMeas(const std::vector<gnss_comm::ObsPtr>& valid_meas,\n                                  const std::vector<gnss_comm::EphemBasePtr>& valid_ephems)\n    {\n        if (!this->isSync()) return;\n        \n        bool flag = false;\n        while (_gnss_meas_buffer.size() > 100)\n        {\n            _gnss_meas_buffer.pop();\n            \n            if (!flag) flag = true;\n        }\n        \n        if (flag)\n            std::cout << \"[GnssSync]: Throw previous Gnss Meas in buffer due to exceeding max size!\" << std::endl;\n        \n        _gnss_meas_buffer.push(std::make_pair(valid_meas, valid_ephems));\n    }\n    \n    void GnssSync::bufferSppMeas(const gnss_comm::gtime_t& gtime,\n                                 const Eigen::Matrix<double, 7, 1>& pos_spp,\n                                 const Eigen::Vector4d& vel_spp)\n    {\n        if (!this->isSync()) return;\n        \n        bool flag = false;\n        while (_spp_meas_buffer.size() > 100)\n        {\n            _spp_meas_buffer.pop();\n            \n            if (!flag) flag = true;\n        }\n        \n        if (flag)\n            std::cout << \"[GnssSync]: Throw previous Spp Meas in buffer due to exceeding max size!\" << std::endl;\n        \n        _spp_meas_buffer.push(SppMeas(gtime, pos_spp, vel_spp));\n    }\n    \n    void GnssSync::storeTimePair(const double& curr_time, const gnss_comm::gtime_t& gnss_time)\n    {\n        if (this->isSync()) return;\n        \n        if (_rosidx_gnsstime.find(curr_time) == _rosidx_gnsstime.end())\n            _rosidx_gnsstime[curr_time] = gnss_time;\n        \n        if (_rosidx_gnsstime.size() >= 3 && _rosidx_header.size() >= 3)\n        {\n            double delta_time = INFINITY;\n            \n            double idx_g, idx_h;\n            \n            for (const auto& item_g : _rosidx_gnsstime)\n                for (const auto& item_h : _rosidx_header)\n                    if (std::fabs(item_g.first - item_h.first) < delta_time)\n                    {\n                        idx_g = item_g.first;\n                        idx_h = item_h.first;\n                        delta_time = std::fabs(idx_g - idx_h);\n                    }\n                    \n            if (delta_time < _unsync_thres)\n            {\n                _gnss2local_time_offset = _rosidx_header.at(idx_h) - gnss_comm::time2sec(_rosidx_gnsstime.at(idx_g)) - (idx_h - idx_g);\n                _isSync = true;\n                \n                std::cout << \"[GnssSync]: gnss time to local time offset = \" << std::setprecision(10) << _gnss2local_time_offset << \" (s) \" << std::endl;\n            }\n            \n            _rosidx_gnsstime.clear();\n            _rosidx_header.clear();\n        }\n    }\n    \n    void GnssSync::storeTimePair(const double& curr_time, const double& header_time)\n    {\n        if (this->isSync()) return;\n        \n        if (_rosidx_header.find(curr_time) == _rosidx_header.end())\n            _rosidx_header[curr_time] = header_time;\n        \n        if (_rosidx_gnsstime.size() >= 3 && _rosidx_header.size() >= 3)\n        {\n            double delta_time = INFINITY;\n            \n            double idx_g, idx_h;\n            \n            for (const auto& item_g : _rosidx_gnsstime)\n                for (const auto& item_h : _rosidx_header)\n                    if (std::fabs(item_g.first - item_h.first) < delta_time)\n                    {\n                        idx_g = item_g.first;\n                        idx_h = item_h.first;\n                        delta_time = std::fabs(idx_g - idx_h);\n                    }\n                    \n            if (delta_time < _unsync_thres)\n            {\n                _gnss2local_time_offset = _rosidx_header.at(idx_h) - gnss_comm::time2sec(_rosidx_gnsstime.at(idx_g)) - (idx_h - idx_g);\n                _isSync = true;\n                        \n                std::cout << \"[GnssSync]: gnss time to local time offset = \" << std::setprecision(10) << _gnss2local_time_offset << \" (s) \" << std::endl;\n            }\n                    \n            _rosidx_gnsstime.clear();\n            _rosidx_header.clear();\n        }\n    }\n    \n    bool GnssSync::getGnssMeasAt(const std_msgs::Header& header, GnssMeas& gnss_meas)\n    {\n        if (!isSync()) return false;\n        \n        double target_time = header.stamp.toSec();\n        \n        bool flag = false;\n        \n        while (!_gnss_meas_buffer.empty())\n        {\n            auto& g_m = _gnss_meas_buffer.front();\n            \n            double g_time = gnss_comm::time2sec(g_m.first[0]->time) + _gnss2local_time_offset;\n            \n            if (g_time < target_time - _unsync_thres)\n            {\n                _gnss_meas_buffer.pop();\n                continue;\n            }\n            \n            if (g_time >= target_time + _unsync_thres)\n                break;\n            \n            std::swap(gnss_meas, g_m);\n            _gnss_meas_buffer.pop();\n            flag = true;\n            \n            break;\n        }    \n            \n        return flag;\n    }\n    \n    bool GnssSync::getSppAt(const std_msgs::Header& header, SppMeas& spp_meas)\n    {        \n        if (!isSync()) return false;\n        \n        double target_time = header.stamp.toSec();\n        \n        bool flag = false;\n        \n        while (!_spp_meas_buffer.empty())\n        {\n            auto& s_m = _spp_meas_buffer.front();\n            \n            double g_time = gnss_comm::time2sec(s_m.gtime) + _gnss2local_time_offset;\n            \n            if (g_time < target_time - _unsync_thres)\n            {\n                _spp_meas_buffer.pop();\n                continue;\n            }\n            \n            if (g_time >= target_time + _unsync_thres)\n                break;\n            \n            std::swap(spp_meas, s_m);\n            _spp_meas_buffer.pop();\n            flag = true;\n            \n            break;\n        }    \n        \n        return flag;\n    }\n    \n    void GnssSync::clearMeasBuffer()\n    {\n        this->clear<GnssMeas>(this->_gnss_meas_buffer);\n        this->clear<SppMeas>(this->_spp_meas_buffer);\n    }\n    \n    void GnssSync::clearMeasBufferUntil(const std_msgs::Header& header)\n    {\n        if (!isSync()) return;\n        \n        double time_end = header.stamp.toSec();\n        \n        while (!_gnss_meas_buffer.empty())\n        {\n            auto& gnss_meas = _gnss_meas_buffer.front();\n            \n            double meas_time = gnss_comm::time2sec(gnss_meas.first[0]->time) + _gnss2local_time_offset;\n            \n            if (meas_time > time_end) break;\n            \n            _gnss_meas_buffer.pop();\n        }\n        \n        while (!_spp_meas_buffer.empty())\n        {\n            auto& spp_meas = _spp_meas_buffer.front();\n            \n            double meas_time = gnss_comm::time2sec(spp_meas.gtime) + _gnss2local_time_offset;\n            \n            if (meas_time > time_end) break;\n            \n            _spp_meas_buffer.pop();\n        }\n    }\n}\n"
  },
  {
    "path": "ingvio_estimator/src/GnssSync.h",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#pragma once\n\n#include <queue>\n\n#include <gnss_comm/gnss_ros.hpp>\n\n#include \"IngvioParams.h\"\n\nnamespace ingvio\n{\n    typedef std::pair<std::vector<gnss_comm::ObsPtr>, \n                      std::vector<gnss_comm::EphemBasePtr>> GnssMeas;\n    \n    struct SppMeas\n    {\n        SppMeas()\n        {\n            posSpp.setZero();\n            velSpp.setZero();\n        }\n        \n        SppMeas(const gnss_comm::gtime_t& time,\n                const Eigen::Matrix<double, 7, 1>& pos_spp,\n                const Eigen::Vector4d& vel_spp) :\n                gtime(time), posSpp(pos_spp), velSpp(vel_spp)\n        {}\n        \n        ~SppMeas() = default;\n        \n        gnss_comm::gtime_t gtime;\n        \n        Eigen::Matrix<double, 7, 1> posSpp;\n        \n        Eigen::Vector4d velSpp;\n    };\n    \n    class GnssSync\n    {\n    public:\n        \n        GnssSync(const IngvioParams& filter_params) : _unsync_thres(0.13)\n        {\n            _isSync = false;\n            \n            if (!filter_params._enable_gnss)\n                return;\n            \n            if (filter_params._use_fix_time_offset)\n            {\n                _isSync = true;\n                _gnss2local_time_offset = filter_params._gnss_local_offset;\n            }\n        }\n        \n        ~GnssSync() = default;\n        \n        GnssSync(const GnssSync&) = delete;\n        \n        GnssSync operator=(const GnssSync&) = delete;\n        \n        void bufferGnssMeas(const std::vector<gnss_comm::ObsPtr>& valid_meas,\n                            const std::vector<gnss_comm::EphemBasePtr>& valid_ephems);\n        \n        void bufferSppMeas(const gnss_comm::gtime_t& gtime,\n                           const Eigen::Matrix<double, 7, 1>& pos_spp,\n                           const Eigen::Vector4d& vel_spp);\n        \n        bool isSync() const\n        {  return _isSync;  }\n        \n        const double& getUnsyncTime() const\n        {  return _gnss2local_time_offset;  }\n        \n        void storeTimePair(const double& curr_time, const gnss_comm::gtime_t& gnss_time);\n        \n        void storeTimePair(const double& curr_time, const double& header_time);\n        \n        bool getGnssMeasAt(const std_msgs::Header& header, GnssMeas& gnss_meas);\n        \n        bool getSppAt(const std_msgs::Header& header, SppMeas& spp_meas);\n        \n        void clearMeasBuffer();\n        \n        void clearMeasBufferUntil(const std_msgs::Header& header);\n        \n    protected:\n        \n        bool _isSync;\n        \n        double _gnss2local_time_offset;\n        \n        double _unsync_thres;\n        \n        std::queue<GnssMeas> _gnss_meas_buffer;\n        \n        std::queue<SppMeas> _spp_meas_buffer;\n        \n        std::map<double, gnss_comm::gtime_t> _rosidx_gnsstime;\n        \n        std::map<double, double> _rosidx_header;\n        \n        template<typename T>\n        static void clear(std::queue<T>& qu)\n        {\n            std::queue<T> empty;\n            std::swap(empty, qu);\n        }\n        \n    };\n}\n"
  },
  {
    "path": "ingvio_estimator/src/GnssUpdate.cpp",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#include <gnss_comm/gnss_spp.hpp>\n\n#include \"StateManager.h\"\n\n#include \"GvioAligner.h\"\n\n#include \"GnssUpdate.h\"\n\n#include \"GnssManager.h\"\n\nnamespace ingvio\n{\n    void GnssUpdate::checkYofStatus(std::shared_ptr<State> state, \n                                    std::shared_ptr<GvioAligner> gvio_aligner)\n    {\n        if (!state->_state_params._enable_gnss || !gvio_aligner->isAlign())\n            return;\n        \n        if (state->_gnss.find(State::GNSSType::YOF) == state->_gnss.end())\n            StateManager::addGNSSVariable(state, State::GNSSType::YOF,\n                                          gvio_aligner->getYawOffset(), \n                                          state->_state_params._init_cov_yof);\n    }\n    \n    \n    void GnssUpdate::getSysInGnssMeas(const GnssMeas& gnss_meas)\n    {\n        _gnss_sys.clear();\n        _gnss_sys.insert(State::GNSSType::YOF);\n        \n        bool flag = false;\n        \n        for (const gnss_comm::ObsPtr& obs : gnss_meas.first)\n            if (_gnss_sys.find(GnssManager::convertSatsys2GnssType(gnss_comm::satsys(obs->sat, NULL))) == _gnss_sys.end())\n            {\n                flag = true;\n                _gnss_sys.insert(GnssManager::convertSatsys2GnssType(gnss_comm::satsys(obs->sat, NULL)));\n            }\n            else\n                continue;\n            \n        if (flag)\n            _gnss_sys.insert(State::GNSSType::FS);\n    }\n    \n    void GnssUpdate::removeUntrackedSys(std::shared_ptr<State> state, \n                                        const GnssMeas& gnss_meas)\n    {\n        if (!state->_state_params._enable_gnss)\n            return;\n        \n        this->getSysInGnssMeas(gnss_meas);\n        \n        std::vector<State::GNSSType> gstate_to_marg;\n        \n        for (auto& gnss_state : state->_gnss)\n            if (_gnss_sys.find(gnss_state.first) == _gnss_sys.end())\n                gstate_to_marg.push_back(gnss_state.first);\n            \n        for (const auto& gs : gstate_to_marg)\n            StateManager::margGNSSVariable(state, gs);\n    }\n    \n    void GnssUpdate::updateTrackedSys(std::shared_ptr<State> state,\n                                      const GnssMeas& gnss_meas,\n                                      std::shared_ptr<GvioAligner> gvio_aligner,\n                                      const std::vector<double>& iono_params)\n    {\n        if (!state->_state_params._enable_gnss || !gvio_aligner->isAlign())\n            return;\n        \n        if (gnss_meas.first.size() <= 0 || iono_params.size() != 8)\n            return;\n        \n        if (!GnssManager::checkGnssStates(state))\n            return;\n        \n        std::vector<gnss_comm::SatStatePtr> all_sat_states =\n            gnss_comm::sat_states(gnss_meas.first, gnss_meas.second);\n        \n        Eigen::Matrix<double, 7, 1> xyzt;\n        \n        xyzt.block<3, 1>(0, 0) = gvio_aligner->getTenu2ecef()*GnssManager::calcTw2enu(state->_gnss.at(State::GNSSType::YOF)->value())*state->_extended_pose->valueTrans1();\n        \n        xyzt.block<4, 1>(3, 0) = GnssManager::getClockbiasVec(state);\n        \n        Eigen::Matrix<double, 4, 1> dopp;\n        \n        dopp.block<3, 1>(0, 0) = gvio_aligner->getRenu2ecef()*GnssManager::calcRw2enu(state->_gnss.at(State::GNSSType::YOF)->value())*state->_extended_pose->valueTrans2();\n        \n        dopp(3, 0) = state->_gnss.at(State::GNSSType::FS)->value();\n        \n        std::vector<Eigen::Vector2d> atmos_delay;\n        std::vector<Eigen::Vector2d> all_sv_azel;\n        \n        Eigen::VectorXd res_pos;\n        Eigen::VectorXd res_vel;\n        Eigen::MatrixXd J_pos_ecef;\n        Eigen::MatrixXd J_vel_ecef;\n        \n        gnss_comm::psr_res(xyzt, gnss_meas.first, all_sat_states, iono_params, res_pos, J_pos_ecef, atmos_delay, all_sv_azel);\n        gnss_comm::dopp_res(dopp, xyzt.block<3, 1>(0, 0), gnss_meas.first, all_sat_states, res_vel, J_vel_ecef);\n        \n        std::vector<std::shared_ptr<Type>> var_order;\n        std::map<std::shared_ptr<Type>, int> local_var_index;\n        \n        var_order.push_back(state->_extended_pose);\n        local_var_index[state->_extended_pose] = 0;\n        \n        var_order.push_back(state->_gnss.at(State::GNSSType::YOF));\n        local_var_index[state->_gnss.at(State::GNSSType::YOF)] = 9;\n        \n        const int max_possible_rows = 2*gnss_meas.first.size();\n        const int max_possible_cols = state->_extended_pose->size() + 6;\n        \n        const Eigen::Matrix3d Rw2ecef = gvio_aligner->getRenu2ecef()*GnssManager::calcRw2enu(state);\n        \n        Eigen::VectorXd res(max_possible_rows);\n        Eigen::MatrixXd H(max_possible_rows, max_possible_cols);\n        Eigen::MatrixXd R(max_possible_rows, max_possible_rows);\n        res.setZero();\n        H.setZero();\n        R.setZero();\n        \n        int row_cnt = 0;\n        int col_cnt = 10;\n        \n        for (int i = 0; i < res_pos.rows(); ++i)\n        {\n            const Eigen::Vector3d unit_rv2sv = -J_pos_ecef.block<1, 3>(i, 0).transpose();\n            const uint32_t sys = gnss_comm::satsys(all_sat_states[i]->sat_id, NULL);\n            \n            if (state->_gnss.find(GnssManager::satsys_to_gnsstype.at(sys)) == state->_gnss.end())\n                continue;\n            \n            auto cb_state = state->_gnss.at(GnssManager::satsys_to_gnsstype.at(sys));\n            \n            Eigen::MatrixXd H_i(1, state->_extended_pose->size()+2);\n            H_i.setZero();\n            \n            H_i.block<1, 3>(0, 0) = unit_rv2sv.transpose()*Rw2ecef*skew(state->_extended_pose->valueTrans1());\n            H_i.block<1, 3>(0, 3) = -unit_rv2sv.transpose()*Rw2ecef;\n            \n            if (_is_adjust_yof)\n            {\n                H_i(0, 9) = -unit_rv2sv.transpose()*gvio_aligner->getRenu2ecef()*GnssManager::dotRw2enu(state)*state->_extended_pose->valueTrans1();\n            }\n            \n            H_i(0, 10) = 1.0;\n            \n            Eigen::VectorXd res_i(1);\n            res_i(0) = -res_pos(i);\n            \n            std::vector<std::shared_ptr<Type>> sub_order(3);\n            \n            sub_order[0] = state->_extended_pose;\n            sub_order[1] = state->_gnss.at(State::GNSSType::YOF);\n            sub_order[2] = cb_state;\n            \n            double ns = gnss_meas.second[i]->ura;\n            int l1_idx = -1;\n            gnss_comm::L1_freq(gnss_meas.first[i], &l1_idx);\n            double npr = gnss_meas.first[i]->psr_std[l1_idx];\n            double sin_el = std::sin(all_sv_azel[i].y());\n            if (std::fabs(sin_el) < 1e-6) sin_el = 1e-6;\n            \n            double psr_noise = _psr_noise_amp*std::pow(ns*npr/(sin_el*sin_el), 0.5);\n            \n \n            if (_is_gnss_chi2_test && !testChiSquared(state, res_i, H_i, sub_order, psr_noise))\n                continue;\n            \n            \n            res(row_cnt, 0) = res_i(0);\n            R(row_cnt, row_cnt) = psr_noise*psr_noise;\n            \n            H.block<1, 9>(row_cnt, local_var_index.at(state->_extended_pose)) = H_i.block<1, 9>(0, 0);\n            H(row_cnt, local_var_index.at(state->_gnss.at(State::GNSSType::YOF))) = H_i(0, 9);\n            \n            if (local_var_index.find(cb_state) == local_var_index.end())\n            {\n                local_var_index[cb_state] = col_cnt;\n                col_cnt += cb_state->size();\n                \n                var_order.push_back(cb_state);\n            }\n            \n            H(row_cnt, local_var_index.at(cb_state)) = 1.0;\n            \n            ++row_cnt;\n        }\n        \n        auto cs_state = state->_gnss.at(State::GNSSType::FS);\n        \n        local_var_index[cs_state] = col_cnt;\n        col_cnt += cs_state->size();\n        \n        var_order.push_back(cs_state);\n        \n        for (int i = 0; i < res_vel.rows(); ++i)\n        {\n            const Eigen::Vector3d unit_rv2sv = -J_vel_ecef.block<1, 3>(i, 0).transpose();\n            const uint32_t sys = gnss_comm::satsys(all_sat_states[i]->sat_id, NULL);\n            \n            if (state->_gnss.find(GnssManager::satsys_to_gnsstype.at(sys)) == state->_gnss.end())\n                continue;\n            \n            std::vector<std::shared_ptr<Type>> sub_order(3);\n            sub_order[0] = state->_extended_pose;\n            sub_order[1] = state->_gnss.at(State::GNSSType::YOF);\n            sub_order[2] = state->_gnss.at(State::GNSSType::FS);\n            \n            Eigen::MatrixXd H_i(1, 11);\n            H_i.setZero();\n            \n            H_i.block<1, 3>(0, 0) = unit_rv2sv.transpose()*Rw2ecef*skew(state->_extended_pose->valueTrans2());\n            H_i.block<1, 3>(0, 6) = -unit_rv2sv.transpose()*Rw2ecef;\n            \n            if (_is_adjust_yof)\n            {\n                H_i(0, 9) = -unit_rv2sv.transpose()*gvio_aligner->getRenu2ecef()*GnssManager::dotRw2enu(state)*state->_extended_pose->valueTrans2();\n            }\n            \n            H_i(0, 10) = 1.0;\n            \n            Eigen::VectorXd res_i(1);\n            res_i(0) = -res_vel(i);\n            \n            double ns = gnss_meas.second[i]->ura;\n            int l1_idx = -1;\n            const double obs_freq = gnss_comm::L1_freq(gnss_meas.first[i], &l1_idx);\n            double ndp = gnss_meas.first[i]->dopp_std[l1_idx]*LIGHT_SPEED/obs_freq;\n            double sin_el = std::sin(all_sv_azel[i].y());\n            if (std::fabs(sin_el) < 1e-6) sin_el = 1e-6;\n            \n            double dopp_noise = _dopp_noise_amp*std::pow(ns*ndp/(sin_el*sin_el), 0.5);\n            \n\n            if (_is_gnss_chi2_test && !testChiSquared(state, res_i, H_i, sub_order, dopp_noise))\n                continue;\n            \n            \n            res(row_cnt, 0) = res_i(0);\n            R(row_cnt, row_cnt) = dopp_noise*dopp_noise;\n            \n            H.block<1, 9>(row_cnt, local_var_index.at(state->_extended_pose)) = H_i.block<1, 9>(0, 0);\n            H(row_cnt, local_var_index.at(state->_gnss.at(State::GNSSType::YOF))) = H_i(0, 9);\n            \n            H(row_cnt, local_var_index.at(state->_gnss.at(State::GNSSType::FS))) = H_i(0, 10);\n            \n            ++row_cnt;\n        }\n        \n        if (row_cnt < max_possible_rows)\n        {\n            res.conservativeResize(row_cnt, 1);\n            \n            R.conservativeResize(row_cnt, row_cnt);\n            \n            H.conservativeResize(row_cnt, H.cols());\n        }\n        \n        if (col_cnt < max_possible_cols)\n            H.conservativeResize(H.rows(), col_cnt);\n        \n        if (res.rows() <= 14 && _is_gnss_strong_reject && !testChiSquared(state, res, H, var_order, R, res.rows()))\n            return;\n         \n        \n        StateManager::ekfUpdate(state, var_order, H, res, R);\n        \n        // GnssManager::printYOF(state);\n    }\n    \n    void GnssUpdate::getSysInSppMeas(const SppMeas& spp_meas)\n    {\n        _spp_sys.clear();\n        \n        if (std::fabs(spp_meas.velSpp(3, 0)) > 1e-03)\n            _spp_sys.insert(State::GNSSType::FS);\n        \n        for (int i = 0; i < 4; ++i)\n            if (std::fabs(spp_meas.posSpp(3+i, 0)) > 1e-03)\n                _spp_sys.insert(GnssManager::idx_to_gnsstype.at(i));\n    }\n    \n    void GnssUpdate::calcSysToAdd(const std::shared_ptr<State> state,\n                                  std::unordered_set<State::GNSSType>& sys_to_add)\n    {\n        sys_to_add.clear();\n        \n        for (const auto& gtype : _spp_sys)\n            if (state->_gnss.find(gtype) == state->_gnss.end())\n                sys_to_add.insert(gtype);\n    }\n    \n    void GnssUpdate::addNewTrackedSys(std::shared_ptr<State> state,\n                                      const GnssMeas& gnss_meas,\n                                      const SppMeas& spp_meas,\n                                      std::shared_ptr<GvioAligner> gvio_aligner,\n                                      const std::vector<double>& iono_params)\n    {\n        if (!state->_state_params._enable_gnss || !gvio_aligner->isAlign())\n            return;\n        \n        if (gnss_meas.first.size() <= 0 || iono_params.size() != 8)\n            return;\n        \n        if (state->_gnss.find(State::GNSSType::YOF) == state->_gnss.end())\n            return;\n        \n        const Eigen::Matrix3d Rw2ecef = gvio_aligner->getRenu2ecef()*GnssManager::calcRw2enu(state);\n        \n        this->getSysInSppMeas(spp_meas);\n        \n        std::unordered_set<State::GNSSType> sys_to_add;\n        this->calcSysToAdd(state, sys_to_add);\n        \n        if (sys_to_add.size() == 0)\n            return;\n        \n        std::vector<gnss_comm::SatStatePtr> all_sat_states =\n        gnss_comm::sat_states(gnss_meas.first, gnss_meas.second);\n        \n        Eigen::Matrix<double, 7, 1> xyzt;\n        \n        xyzt.block<3, 1>(0, 0) = gvio_aligner->getTenu2ecef()*GnssManager::calcTw2enu(state->_gnss.at(State::GNSSType::YOF)->value())*state->_extended_pose->valueTrans1();\n        \n        xyzt.block<4, 1>(3, 0) = GnssManager::getClockbiasVec(state);\n        \n        for (const auto& item : sys_to_add)\n            if (item != State::GNSSType::FS)\n            {\n                int idx_to_add = 3 + gnss_comm::sys2idx.at(GnssManager::gnsstype_to_satsys.at(item));\n                xyzt(idx_to_add, 0) = spp_meas.posSpp(idx_to_add, 0);\n            }\n        \n        Eigen::Matrix<double, 4, 1> dopp = Eigen::Matrix<double, 4, 1>::Zero();\n        \n        dopp.block<3, 1>(0, 0) = gvio_aligner->getRenu2ecef()*GnssManager::calcRw2enu(state->_gnss.at(State::GNSSType::YOF)->value())*state->_extended_pose->valueTrans2();\n        \n        if (sys_to_add.find(State::GNSSType::FS) == sys_to_add.end())\n        {\n            if (state->_gnss.find(State::GNSSType::FS) == state->_gnss.end())\n                std::cout << \"[GnssUpdate]: Fs is either added or in the state!\" << std::endl;\n            else\n                dopp(3, 0) = state->_gnss.at(State::GNSSType::FS)->value();\n        }\n        else\n            dopp(3, 0) = spp_meas.velSpp(3, 0);\n        \n        std::vector<Eigen::Vector2d> atmos_delay;\n        std::vector<Eigen::Vector2d> all_sv_azel;\n        \n        Eigen::VectorXd res_pos;\n        Eigen::VectorXd res_vel;\n        Eigen::MatrixXd J_pos_ecef;\n        Eigen::MatrixXd J_vel_ecef;\n        \n        gnss_comm::psr_res(xyzt, gnss_meas.first, all_sat_states, iono_params, res_pos, J_pos_ecef, atmos_delay, all_sv_azel);\n        gnss_comm::dopp_res(dopp, xyzt.block<3, 1>(0, 0), gnss_meas.first, all_sat_states, res_vel, J_vel_ecef);\n    \n        \n        for (const auto& gtype : sys_to_add)\n            if (gtype == State::GNSSType::FS)\n            {\n                Eigen::VectorXd res = -res_vel;\n                \n                Eigen::MatrixXd Hf(res.rows(), 1);\n                Hf.setOnes();\n                \n                Eigen::MatrixXd Hx(res.rows(), state->_extended_pose->size()+1);\n                Hx.setZero();\n                \n                Eigen::MatrixXd batch_unit_rv2sv_T = -J_vel_ecef.block(0, 0, J_vel_ecef.rows(), 3);\n                \n                Hx.block(0, 0, Hx.rows(), 3) = batch_unit_rv2sv_T*Rw2ecef*skew(state->_extended_pose->valueTrans2());\n                \n                Hx.block(0, 6, Hx.rows(), 3) = -batch_unit_rv2sv_T*Rw2ecef;\n                \n                if (_is_adjust_yof)\n                {\n                    Hx.block(0, 9, Hx.rows(), 1) = -batch_unit_rv2sv_T*gvio_aligner->getRecef2enu()*GnssManager::dotRw2enu(state)*state->_extended_pose->valueTrans2();\n                }\n                \n                std::vector<std::shared_ptr<Type>> x_order(2);\n                x_order[0] = state->_extended_pose;\n                x_order[1] = state->_gnss.at(State::GNSSType::YOF);\n                \n                std::shared_ptr<Scalar> fs = std::make_shared<Scalar>();\n                fs->setValue(spp_meas.velSpp(3, 0));\n                \n                double avg_noise = 0.0;\n                \n                for (int i = 0; i < gnss_meas.second.size(); ++i)\n                {\n                    double ns = gnss_meas.second[i]->ura;\n                    int l1_idx = -1;\n                    const double obs_freq = gnss_comm::L1_freq(gnss_meas.first[i], &l1_idx);\n                    double ndp = gnss_meas.first[i]->dopp_std[l1_idx]*LIGHT_SPEED/obs_freq;\n                    double sin_el = std::sin(all_sv_azel[i].y());\n                    if (std::fabs(sin_el) < 1e-6) sin_el = 1e-6;\n                    \n                    avg_noise += ns*ndp/(sin_el*sin_el);\n                }\n                \n                avg_noise /= gnss_meas.second.size();\n                avg_noise = _dopp_noise_amp*std::sqrt(avg_noise);\n                \n                if (!StateManager::addVariableDelayed(state, fs, x_order, Hx, Hf, res,\n                    avg_noise, 0.95, true))\n                    continue;\n                \n                state->_gnss[State::GNSSType::FS] = fs;\n            }\n            else\n            {\n                Eigen::VectorXd res;\n                Eigen::MatrixXd batch_unit_rv2sv_T;\n                double avg_noise;\n                \n                this->getResJacobianOfSys(gtype, gnss_meas, all_sv_azel,\n                                          res_pos, J_pos_ecef,\n                                          res, batch_unit_rv2sv_T, avg_noise);\n                \n                avg_noise *= _psr_noise_amp;\n                \n                Eigen::MatrixXd Hf(res.rows(), 1);\n                Hf.setOnes();\n                \n                std::vector<std::shared_ptr<Type>> x_order(2);\n                x_order[0] = state->_extended_pose;\n                x_order[1] = state->_gnss.at(State::GNSSType::YOF);\n                \n                Eigen::MatrixXd Hx(res.rows(), 10);\n                Hx.setZero();\n                \n                Hx.block(0, 0, Hx.rows(), 3) = batch_unit_rv2sv_T*Rw2ecef*skew(state->_extended_pose->valueTrans1());\n                \n                Hx.block(0, 3, Hx.rows(), 3) = -batch_unit_rv2sv_T*Rw2ecef;\n                \n                if (_is_adjust_yof)\n                {\n                    Hx.block(0, 9, Hx.rows(), 1) = -batch_unit_rv2sv_T*gvio_aligner->getRecef2enu()*GnssManager::dotRw2enu(state)*state->_extended_pose->valueTrans1();\n                }\n                \n                std::shared_ptr<Scalar> cb = std::make_shared<Scalar>();\n                cb->setValue(spp_meas.posSpp(3+gnss_comm::sys2idx.at(GnssManager::gnsstype_to_satsys.at(gtype)), 0));\n                \n                if (!StateManager::addVariableDelayed(state, cb, x_order, Hx, Hf, res,\n                    avg_noise, 0.95, true))\n                    continue;\n                \n                state->_gnss[gtype] = cb;\n            }\n    }\n    \n    void GnssUpdate::getResJacobianOfSys(const State::GNSSType& gtype,\n                                         const GnssMeas& gnss_meas,\n                                         const std::vector<Eigen::Vector2d>& all_sv_azel,\n                                         const Eigen::VectorXd& res_total,\n                                         const Eigen::MatrixXd& H_total,\n                                         Eigen::VectorXd& res_sys,\n                                         Eigen::MatrixXd& batch_unit_rv2sv_T_sys,\n                                         double& noise)\n    {\n        assert(H_total.cols() == 7);\n        \n        const int col_idx = 3 + gnss_comm::sys2idx.at(GnssManager::gnsstype_to_satsys.at(gtype));\n        \n        std::vector<int> this_sys_row;\n        \n        for (int i = 0; i < H_total.rows(); ++i)\n            if (H_total(i, col_idx) == 1.0)\n                this_sys_row.push_back(i);\n            \n        res_sys = Eigen::VectorXd::Zero(this_sys_row.size());\n        batch_unit_rv2sv_T_sys = Eigen::MatrixXd::Zero(this_sys_row.size(), 3);\n        \n        int row_cnt = 0;\n        noise = 0.0;\n        for (const int& row_idx : this_sys_row)\n        {\n            res_sys(row_cnt) = -res_total(row_idx);\n            batch_unit_rv2sv_T_sys.row(row_cnt) = -H_total.block<1, 3>(row_idx, 0);\n            \n            ++row_cnt;\n            \n            double ns = gnss_meas.second[row_idx]->ura;\n            int l1_idx = -1;\n            gnss_comm::L1_freq(gnss_meas.first[row_idx], &l1_idx);\n            double npr = gnss_meas.first[row_idx]->psr_std[l1_idx];\n            double sin_el = std::sin(all_sv_azel[row_idx].y());\n            if (std::fabs(sin_el) < 1e-6) sin_el = 1e-6;\n            \n            noise += ns*npr/(sin_el*sin_el);\n        }\n        \n        noise /= this_sys_row.size();\n        noise = std::sqrt(noise);\n    }\n}\n\n"
  },
  {
    "path": "ingvio_estimator/src/GnssUpdate.h",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#pragma once\n\n#include <memory>\n#include <unordered_set>\n\n#include \"State.h\"\n#include \"Update.h\"\n\n#include \"IngvioParams.h\"\n\n#include \"GnssSync.h\"\n\nnamespace ingvio\n{\n    class State;\n    class GvioAligner;\n    \n    class GnssUpdate : public UpdateBase\n    {\n    public:\n        using UpdateBase::UpdateBase;\n        \n        GnssUpdate(const IngvioParams& filter_params) :\n        UpdateBase(filter_params._chi2_max_dof, filter_params._chi2_thres),\n        _psr_noise_amp(filter_params._psr_noise_amp),\n        _dopp_noise_amp(filter_params._dopp_noise_amp),\n        _is_adjust_yof(filter_params._is_adjust_yof),\n        _is_gnss_chi2_test(filter_params._is_gnss_chi2_test),\n        _is_gnss_strong_reject(filter_params._is_gnss_strong_reject)\n        {}\n     \n        virtual ~GnssUpdate() {}\n        \n        GnssUpdate(const GnssUpdate&) = delete;\n        \n        GnssUpdate operator=(const GnssUpdate&) = delete;\n        \n        void checkYofStatus(std::shared_ptr<State> state, \n                            std::shared_ptr<GvioAligner> gvio_aligner);\n        \n        void removeUntrackedSys(std::shared_ptr<State> state, \n                                const GnssMeas& gnss_meas);\n        \n        void updateTrackedSys(std::shared_ptr<State> state,\n                              const GnssMeas& gnss_meas,\n                              std::shared_ptr<GvioAligner> gvio_aligner,\n                              const std::vector<double>& iono_params);\n        \n        void addNewTrackedSys(std::shared_ptr<State> state,\n                              const GnssMeas& gnss_meas,\n                              const SppMeas& spp_meas,\n                              std::shared_ptr<GvioAligner> gvio_aligner,\n                              const std::vector<double>& iono_params);\n        \n    protected:\n        \n        double _psr_noise_amp = 1.0;\n        \n        double _dopp_noise_amp = 1.0;\n        \n        int _is_adjust_yof = 0;\n        \n        int _is_gnss_chi2_test = 0;\n        \n        int _is_gnss_strong_reject = 0;\n        \n        void getSysInGnssMeas(const GnssMeas& gnss_meas);\n        \n        void getSysInSppMeas(const SppMeas& spp_meas);\n        \n        void calcSysToAdd(const std::shared_ptr<State> state,\n                          std::unordered_set<State::GNSSType>& sys_to_add);\n        \n        void getResJacobianOfSys(const State::GNSSType& gtype,\n                                 const GnssMeas& gnss_meas,\n                                 const std::vector<Eigen::Vector2d>& all_sv_azel,\n                                 const Eigen::VectorXd& res_total,\n                                 const Eigen::MatrixXd& H_total,\n                                 Eigen::VectorXd& res_sys,\n                                 Eigen::MatrixXd& batch_unit_rv2sv_T_sys,\n                                 double& noise);\n        \n        std::unordered_set<State::GNSSType> _gnss_sys;\n        std::unordered_set<State::GNSSType> _spp_sys;\n        \n    };\n}\n"
  },
  {
    "path": "ingvio_estimator/src/GvioAligner.cpp",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#include <gnss_comm/gnss_spp.hpp>\n\n#include \"PoseState.h\"\n\n#include \"Color.h\"\n\n#include \"GvioAligner.h\"\n\nnamespace ingvio\n{\n    bool GvioAligner::isAlign() const\n    {  return _isAligned;  }\n    \n    Eigen::Isometry3d GvioAligner::getTecef2w() const\n    {\n        Eigen::Isometry3d T_enu2w = Eigen::Isometry3d::Identity();\n        T_enu2w.linear() = this->getRenu2w();\n        \n        return T_enu2w*this->getTecef2enu();\n    }\n    \n    Eigen::Isometry3d GvioAligner::getTw2ecef() const\n    {\n        Eigen::Isometry3d T_w2enu = Eigen::Isometry3d::Identity();\n        T_w2enu.linear() = this->getRw2enu();\n        \n        return this->getTenu2ecef()*T_w2enu;\n    }\n    \n    double GvioAligner::getYawOffset() const\n    {\n        return _yaw_offset;\n    }\n    \n    Eigen::Isometry3d GvioAligner::getTecef2enu() const\n    {\n        return _T_enu2ecef.inverse();\n    }\n    \n    Eigen::Isometry3d GvioAligner::getTenu2ecef() const\n    {\n        return _T_enu2ecef;\n    }\n    \n    Eigen::Matrix3d GvioAligner::getRecef2enu() const\n    {\n        return _T_enu2ecef.linear().transpose();\n    }\n    \n    Eigen::Matrix3d GvioAligner::getRenu2ecef() const\n    {\n        return _T_enu2ecef.linear();\n    }\n    \n    Eigen::Matrix3d GvioAligner::getRw2enu() const\n    {\n        return Eigen::AngleAxisd(_yaw_offset, Eigen::Vector3d::UnitZ()).toRotationMatrix();\n    }\n    \n    Eigen::Matrix3d GvioAligner::getRenu2w() const\n    {\n        return this->getRw2enu().transpose();\n    }\n    \n    void GvioAligner::batchAlign(const GnssMeas& gnss_meas, \n                                 const std::shared_ptr<SE23> epose, \n                                 const std::vector<double>& iono)\n    {\n        if (_isAligned) return;\n        \n        _iono_params.clear();\n        std::copy(iono.begin(), iono.end(), std::back_inserter(_iono_params));\n        \n        if (_align_buffer.size() < _batch_size)\n            _align_buffer.push_back(std::make_tuple(epose->valueTrans1(),\n                                                    epose->valueTrans2(),\n                                                    gnss_meas.first,\n                                                    gnss_meas.second));\n        else\n        {\n            Eigen::Vector2d hor_vel = Eigen::Vector2d::Zero();\n            \n            std::for_each(_align_buffer.begin(), _align_buffer.end(), \n                [&hor_vel](const AlignBufferItem& item)\n                {\n                    hor_vel += std::get<velVIO>(item).head<2>().cwiseAbs();\n                }\n            );\n            \n            hor_vel /= _align_buffer.size();\n            \n            if (hor_vel.norm() <= _vel_thres)\n            {\n                std::cout << color::setYellow << \"[GvioAligner]: Horizontal velocity excitation not enough, waiting and restart ...\" << color::resetColor << std::endl;\n                \n                _align_buffer.clear();\n                _isAligned = false;\n                \n                _num_all_meas = 0;\n                _all_sat_states.clear();\n                \n                return;\n            }\n            \n            std::cout << color::setBlue << \"[GvioAligner]: Start batch alignment ...\" << color::resetColor << std::endl;\n            \n            _num_all_meas = 0;\n            _all_sat_states.clear();\n            \n            for (const auto& align_item : _align_buffer)\n            {\n                _all_sat_states.push_back(\n                    gnss_comm::sat_states(std::get<obsGNSS>(align_item), \n                                          std::get<ephemGNSS>(align_item)));\n                \n                _num_all_meas += std::get<obsGNSS>(align_item).size();\n            }\n            \n            Eigen::Matrix<double, 7, 1> rough_anchor_ecef;\n            rough_anchor_ecef.setZero();\n            \n            if (!coarseLocalization(rough_anchor_ecef))\n            {\n                _align_buffer.clear();\n                _isAligned = false;\n                \n                _num_all_meas = 0;\n                _all_sat_states.clear();\n                \n                return;\n            }\n            \n            double yaw_offset = 0.0;\n            double rcv_ddt = 0.0;\n            \n            if (!yawAlignment(rough_anchor_ecef.head<3>(), yaw_offset, rcv_ddt))\n            {\n                _align_buffer.clear();\n                _isAligned = false;\n                \n                _num_all_meas = 0;\n                _all_sat_states.clear();\n                \n                return;\n            }\n            \n            Eigen::Matrix<double, 7, 1> refined_anchor_ecef;\n            refined_anchor_ecef.setZero();\n            \n            if (!anchorRefinement(yaw_offset, rcv_ddt, rough_anchor_ecef, refined_anchor_ecef))\n            {\n                _align_buffer.clear();\n                _isAligned = false;\n                \n                _num_all_meas = 0;\n                _all_sat_states.clear();\n                \n                return;\n            }\n            \n            _align_buffer.clear();\n            _num_all_meas = 0;\n            _all_sat_states.clear();\n            _isAligned = true;\n            \n            Eigen::Matrix3d R_enu2ecef = gnss_comm::ecef2rotation(refined_anchor_ecef.head<3>());\n            \n            _T_enu2ecef.linear() = R_enu2ecef;\n            _T_enu2ecef.translation() = refined_anchor_ecef.head<3>();\n            \n            _yaw_offset = yaw_offset;\n            \n            std::cout << color::setGreen << \"[GvioAligner]: Yaw offset from north = \" << yaw_offset*180.0/M_PI << \" (deg)\" << color::resetColor << std::endl;\n            \n            std::cout << color::setGreen << \"[GvioAligner]: Refined anchor in ECEF = \" << refined_anchor_ecef.head<3>().transpose() << \" (m)\" << color::resetColor << std::endl;\n        }\n    }\n    \n    bool GvioAligner::coarseLocalization(Eigen::Matrix<double, 7, 1>& rough_anchor_ecef)\n    {\n        rough_anchor_ecef.setZero();\n        \n        std::vector<gnss_comm::ObsPtr> accumObs;\n        std::vector<gnss_comm::EphemBasePtr> accumEphems;\n        \n        for (const auto& align_item : _align_buffer)\n        {\n            std::copy(std::get<obsGNSS>(align_item).begin(),\n                      std::get<obsGNSS>(align_item).end(),\n                      std::back_inserter(accumObs));\n            \n            std::copy(std::get<ephemGNSS>(align_item).begin(),\n                      std::get<ephemGNSS>(align_item).end(),\n                      std::back_inserter(accumEphems));\n        }\n        \n        Eigen::Matrix<double, 7, 1> xyzt = gnss_comm::psr_pos(accumObs, accumEphems, this->_iono_params);\n        \n        if (xyzt.hasNaN() || xyzt.topRows<3>().norm() < 1e-06)\n        {\n            std::cout << color::setRed << \"[GvioAligner]: Coarse anchor localization failed!\" << color::resetColor << std::endl;\n            return false;\n        }\n        \n        for (int i = 0; i < 4; ++i)\n            if (std::fabs(xyzt(i+3, 0)) < 1.0)\n                xyzt(i+3, 0) = 0.0;\n            \n        rough_anchor_ecef = xyzt;\n        return true;\n    }\n    \n    bool GvioAligner::yawAlignment(const Eigen::Vector3d& rough_anchor_ecef, \n                                   double& yaw_offset,\n                                   double& rcv_ddt)\n    {\n        yaw_offset = 0.0;\n        rcv_ddt = 0.0;\n        \n        double estYaw = 0.0;\n        double estRcvDdt = 0.0;\n        \n        const Eigen::Matrix3d roughRenu2ecef = gnss_comm::ecef2rotation(rough_anchor_ecef);\n        \n        int iter = 0;\n        \n        double delta_yaw_norm = 1.0;\n        \n        while (iter <= _max_iter && delta_yaw_norm > _conv_epsilon)\n        {\n            Eigen::MatrixXd A(_num_all_meas, 2);\n            A.setZero();\n            A.col(1).setOnes();\n            \n            Eigen::VectorXd b(_num_all_meas);\n            b.setZero();\n            \n            Eigen::Matrix3d Rw2enu(Eigen::AngleAxisd(estYaw, Eigen::Vector3d::UnitZ()));\n            \n            Eigen::Matrix3d dotC3;\n            dotC3 << -std::sin(estYaw), -std::cos(estYaw), 0.0,\n                      std::cos(estYaw), -std::sin(estYaw), 0.0,\n                                   0.0,               0.0, 0.0;\n                                   \n            int row_cnt = 0;\n            for (int i = 0; i < _align_buffer.size(); ++i)\n            {\n                auto& align_item = _align_buffer[i]; \n                \n                Eigen::Vector4d vel_ddt_ecef;\n                \n                vel_ddt_ecef.head<3>() = roughRenu2ecef*Rw2enu*std::get<velVIO>(align_item);\n                vel_ddt_ecef(3) = estRcvDdt;\n                \n                Eigen::VectorXd epochRes;\n                Eigen::MatrixXd epochJ; \n                \n                gnss_comm::dopp_res(vel_ddt_ecef, rough_anchor_ecef, \n                                    std::get<obsGNSS>(align_item), _all_sat_states[i],\n                                    epochRes, epochJ);\n                \n                b.segment(row_cnt, std::get<obsGNSS>(align_item).size()) = epochRes;\n                A.block(row_cnt, 0, std::get<obsGNSS>(align_item).size(), 1) = \n                epochJ.leftCols(3)*roughRenu2ecef*dotC3*std::get<velVIO>(align_item);\n                \n                row_cnt += std::get<obsGNSS>(align_item).size();\n            }\n            \n            Eigen::VectorXd delta_yaw = -(A.transpose()*A).inverse()*A.transpose()*b;\n            \n            estYaw += delta_yaw(0);\n            estRcvDdt += delta_yaw(1);\n            \n            delta_yaw_norm = delta_yaw.norm();\n            \n            ++iter;\n        }\n        \n        if (iter > _max_iter)\n        {\n            std::cout << color::setRed << \"[GvioAligner]: Yaw alignment reaches max iter, failed!\" << color::resetColor << std::endl;\n            \n            return false;\n        }\n        \n        yaw_offset = estYaw;\n        if (yaw_offset > M_PI)\n            yaw_offset -= std::floor(estYaw/(2.0*M_PI)+0.5)*(2.0*M_PI);\n        else if (yaw_offset < -M_PI)\n            yaw_offset -= std::ceil(estYaw/(2.0*M_PI)-0.5)*(2.0*M_PI);\n        \n        rcv_ddt = estRcvDdt;\n        \n        return true;\n    }\n    \n    bool GvioAligner::anchorRefinement(const double& yaw_offset,\n                                       const double& rcv_ddt,\n                                       const Eigen::Matrix<double, 7, 1>& rough_anchor_ecef,\n                                       Eigen::Matrix<double, 7, 1>& refined_anchor_ecef)\n    {\n        refined_anchor_ecef = rough_anchor_ecef;\n        \n        const Eigen::Matrix3d alignedRw2enu = Eigen::AngleAxisd(yaw_offset, Eigen::Vector3d::UnitZ()).toRotationMatrix();\n        \n        auto getRefinedRw2ecef = [&alignedRw2enu](const Eigen::Matrix<double, 7, 1>& anchor_ecef)\n        {\n            return gnss_comm::ecef2rotation(anchor_ecef.head<3>())*alignedRw2enu;\n        };\n        \n        std::vector<Eigen::Matrix<double, 7, 1>> spp(_align_buffer.size());\n        \n        for (int i = 0; i < _align_buffer.size(); ++i)\n        {\n            const auto& align_item = _align_buffer[i];\n            \n            Eigen::Matrix<double, 7, 1> xyzt = gnss_comm::psr_pos(\n                std::get<obsGNSS>(align_item),\n                std::get<ephemGNSS>(align_item),\n                this->_iono_params);\n            \n            if (xyzt.hasNaN() || xyzt.head<3>().norm() < 1e-03)\n            {\n                std::cout << color::setRed << \"[GvioAligner]: Anchor refinement failure due to unable to conduct SPP!\" << color::resetColor << std::endl;\n                \n                return false;\n            }\n            \n            spp[i] = xyzt;\n        }\n        \n        int iter_refine = 0;\n        \n        while (iter_refine <= _max_iter)\n        {\n            Eigen::Vector3d anchor_pos = Eigen::Vector3d::Zero();\n            \n            for (int i = 0; i < _align_buffer.size(); ++i)\n                anchor_pos += spp[i].head<3>() - getRefinedRw2ecef(refined_anchor_ecef)*std::get<posVIO>(_align_buffer[i]);\n            \n            anchor_pos /= _align_buffer.size();\n            \n            Eigen::Vector3d dx = anchor_pos - refined_anchor_ecef.head<3>();\n            \n            refined_anchor_ecef.head<3>() = anchor_pos;\n            \n            if (dx.norm() > _conv_epsilon)\n                break;\n            \n            ++iter_refine;\n        }\n        \n        if (iter_refine > _max_iter)\n        {\n            std::cout << color::setRed << \"[GvioAligner]: Anchor refinement failure reaching max iter!\" << color::resetColor << std::endl;\n            \n            return false;\n        }\n        \n        refined_anchor_ecef.tail<4>() = spp.rbegin()->tail<4>();\n        \n        return true;\n    }\n}\n"
  },
  {
    "path": "ingvio_estimator/src/GvioAligner.h",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#pragma once\n\n#include <tuple>\n\n#include <Eigen/Core>\n#include <Eigen/Geometry>\n\n#include \"IngvioParams.h\"\n\n#include \"GnssData.h\"\n#include \"GnssSync.h\"\n\nnamespace ingvio\n{\n    class SE23;\n    \n    class GvioAligner\n    {\n    public:\n        \n        GvioAligner(const IngvioParams& filter_params)\n        {\n            _batch_size = filter_params._gv_align_batch_size;\n            _max_iter = filter_params._gv_align_max_iter;\n            _conv_epsilon = filter_params._gv_align_conv_epsilon;\n            _vel_thres = filter_params._gv_align_vel_thres;\n        }\n        \n        ~GvioAligner() = default;\n\n        GvioAligner(const GvioAligner&) = delete;\n        GvioAligner operator=(const GvioAligner&) = delete;\n        \n        bool isAlign() const;\n        \n        Eigen::Isometry3d getTecef2w() const;\n        \n        Eigen::Isometry3d getTw2ecef() const;\n        \n        Eigen::Isometry3d getTecef2enu() const;\n        \n        Eigen::Isometry3d getTenu2ecef() const;\n\n        double getYawOffset() const;\n        \n        Eigen::Matrix3d getRecef2enu() const;\n        \n        Eigen::Matrix3d getRenu2ecef() const;\n        \n        Eigen::Matrix3d getRw2enu() const;\n        \n        Eigen::Matrix3d getRenu2w() const;\n        \n        void batchAlign(const GnssMeas& gnss_meas, \n                        const std::shared_ptr<SE23> epose, \n                        const std::vector<double>& iono);\n        \n    protected:\n        \n        int _batch_size;\n        int _max_iter;\n        double _conv_epsilon;       \n        double _vel_thres;\n        \n        bool _isAligned = false;\n        \n        Eigen::Isometry3d _T_enu2ecef;\n        double _yaw_offset;\n        \n        typedef std::tuple<Eigen::Vector3d, Eigen::Vector3d, std::vector<gnss_comm::ObsPtr>,\n        std::vector<gnss_comm::EphemBasePtr>> AlignBufferItem;\n        \n        enum AlignBufferIndex {posVIO, velVIO, obsGNSS, ephemGNSS};\n        \n        std::vector<AlignBufferItem> _align_buffer;\n        std::vector<double> _iono_params;\n        \n        int _num_all_meas;\n        \n        std::vector<std::vector<gnss_comm::SatStatePtr>> _all_sat_states;\n        \n        bool coarseLocalization(Eigen::Matrix<double, 7, 1>& rough_anchor_ecef);\n        \n        bool yawAlignment(const Eigen::Vector3d& rough_anchor_ecef, \n                          double& yaw_offset,\n                          double& rcv_ddt);\n        \n        bool anchorRefinement(const double& yaw_offset,\n                              const double& rcv_ddt,\n                              const Eigen::Matrix<double, 7, 1>& rough_anchor_ecef,\n                              Eigen::Matrix<double, 7, 1>& refined_anchor_ecef);\n    };\n    \n}\n"
  },
  {
    "path": "ingvio_estimator/src/ImuPropagator.cpp",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#include <algorithm>\n\n#include \"StateManager.h\"\n#include \"State.h\"\n#include \"ImuPropagator.h\"\n\nnamespace ingvio\n{\n    void ImuPropagator::storeImu(const ImuCtrl& imu_ctrl)\n    {\n        if (_imu_ctrl_buffer.size() > _max_imu_buffer_size)\n        {\n            std::cout << \"[ImuPropagator]: Exceeding imu max buffer size, throw curr imu ctrl!\" << std::endl;\n            return;\n        }\n        else\n            _imu_ctrl_buffer.push_back(imu_ctrl);\n        \n        if (!_has_gravity_set && _init_imu_buffer_sp > 0)\n        {\n            if (_imu_ctrl_buffer.size() < _init_imu_buffer_sp) return;\n            \n            std::cout << \"[ImuPropagator]: Start init gravity norm ...\" << std::endl;\n            \n            Eigen::Vector3d sum_gravity = Eigen::Vector3d::Zero();\n            \n            for (const auto& item : _imu_ctrl_buffer)\n                sum_gravity += item._accel_raw;\n            \n            sum_gravity /= _imu_ctrl_buffer.size();\n            \n            if (std::fabs(sum_gravity.norm()-_init_gravity)/_init_gravity > 0.02)\n            {\n                std::cout << \"[ImuPropagator]: Keep Camera STEADY!! Reinit gravity ...\" << std::endl;\n                \n                _imu_ctrl_buffer.clear();\n                _has_gravity_set = false;\n            }\n            else\n            {\n                _gravity = Eigen::Vector3d(0.0, 0.0, -sum_gravity.norm());\n                \n                _quat_init = Eigen::Quaterniond::FromTwoVectors(-sum_gravity, _gravity);\n                \n                _has_gravity_set = true;\n                \n                std::cout << \"[ImuPropagator]: Gravity in body init: gx = \" << -sum_gravity.x() << \" gy = \" << -sum_gravity.y() << \" gz = \" << -sum_gravity.z() << std::endl;\n            }\n        }\n    }\n    \n    bool ImuPropagator::getAvgQuat(Eigen::Quaterniond& quat_avg, int num_ctrls)\n    {\n        if (_imu_ctrl_buffer.size() == 0 || num_ctrls <= 0)\n        {\n            quat_avg.setIdentity();\n            return false;\n        }\n        \n        Eigen::Vector3d sum_sf = Eigen::Vector3d::Zero();\n        \n        int idx = 0;\n        \n        for (int i = _imu_ctrl_buffer.size()-1; i >= std::max(0, (int)_imu_ctrl_buffer.size()-num_ctrls); --i)\n        {\n            sum_sf += _imu_ctrl_buffer[i]._accel_raw;\n            ++idx;\n        }\n        \n        sum_sf /= idx;\n        sum_sf.normalize();\n        \n        quat_avg = Eigen::Quaterniond::FromTwoVectors(-sum_sf, Eigen::Vector3d(0, 0, -1));\n        \n        return true;\n    }\n    \n    void ImuPropagator::stateAndCovTransition(std::shared_ptr<State> state,\n                                              const ImuCtrl& imu_ctrl,\n                                              double dt,\n                                              Eigen::Matrix<double, 15, 15>& Phi,\n                                              Eigen::Matrix<double, 15, 12>& G,\n                                              bool isAnalytic)\n    {        \n        Phi.setIdentity();\n        G.setZero();\n        \n        Eigen::Matrix3d R_hat = state->_extended_pose->valueLinearAsMat();\n        Eigen::Vector3d p_hat = state->_extended_pose->valueTrans1();\n        Eigen::Vector3d v_hat = state->_extended_pose->valueTrans2();\n        \n        G.block<3, 3>(0, 0) = R_hat;\n        G.block<3, 3>(3, 0) = skew(p_hat)*R_hat;\n        G.block<3, 3>(6, 0) = skew(v_hat)*R_hat;\n        G.block<3, 3>(6, 3) = R_hat;\n        G.block<3, 3>(9, 6) = Eigen::Matrix3d::Identity();\n        G.block<3, 3>(12, 9) = Eigen::Matrix3d::Identity();\n        \n        if (isAnalytic)\n        {\n            const Eigen::Vector3d gyro_unbiased = imu_ctrl._gyro_raw - state->_bg->value();\n            const Eigen::Vector3d acc_unbiased = imu_ctrl._accel_raw - state->_ba->value();\n            \n            state->_timestamp += dt;\n            \n            Eigen::Matrix3d Gamma0 = GammaFunc(dt*gyro_unbiased, 0);\n            Eigen::Matrix3d Gamma1 = GammaFunc(dt*gyro_unbiased, 1);\n            Eigen::Matrix3d Gamma2 = GammaFunc(dt*gyro_unbiased, 2);\n            \n            Eigen::Matrix3d R_hat_new = R_hat*Gamma0;\n            state->_extended_pose->setValueLinearByMat(R_hat_new);\n            \n            Eigen::Vector3d v_hat_new = v_hat + _gravity*dt + R_hat*Gamma1*acc_unbiased*dt;\n            state->_extended_pose->setValueTrans2(v_hat_new);\n            \n            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);\n            state->_extended_pose->setValueTrans1(p_hat_new);\n            \n            if (state->_state_params._enable_gnss)\n                for (int i = 0; i < 4; ++i)\n                    if (state->_gnss.find(static_cast<State::GNSSType>(i)) != state->_gnss.end() && state->_gnss.find(State::GNSSType::FS) != state->_gnss.end())\n                    {\n                        double clockbias_hat = state->_gnss.at(static_cast<State::GNSSType>(i))->value();\n                        \n                        double clockbias_hat_new = clockbias_hat + dt*state->_gnss.at(State::GNSSType::FS)->value();\n                        \n                        state->_gnss.at(static_cast<State::GNSSType>(i))->setValue(clockbias_hat_new);\n                    }\n            \n            Phi.block<3, 3>(3, 0) = 0.5*skew(_gravity)*std::pow(dt, 2);\n            Phi.block<3, 3>(3, 6) = dt*Eigen::Matrix3d::Identity();\n            Phi.block<3, 3>(6, 0) = skew(_gravity)*dt;\n            \n            Phi.block<3, 3>(0, 9) = -R_hat*Gamma1*dt;\n            Phi.block<3, 3>(6, 12) = -R_hat*Gamma1*dt;\n            \n            Phi.block<3, 3>(3, 12) = -R_hat*Gamma2*std::pow(dt, 2);\n            \n            Phi.block<3, 3>(6, 9) = -skew(v_hat_new)*R_hat*Gamma1*dt + R_hat*Psi1Func(gyro_unbiased, acc_unbiased, dt);\n            \n            Phi.block<3, 3>(3, 9) = -skew(p_hat_new)*R_hat*Gamma1*dt + R_hat*Psi2Func(gyro_unbiased, acc_unbiased, dt);\n        }\n        else\n        {\n            const Eigen::Vector3d gyro_unbiased = imu_ctrl._gyro_raw - state->_bg->value();\n            const Eigen::Vector3d acc_unbiased = imu_ctrl._accel_raw - state->_ba->value();\n            \n            state->_timestamp += dt;\n            \n            Eigen::Quaterniond q_hat = state->_extended_pose->valueLinearAsQuat();\n            \n            Eigen::Vector3d delta_angle = dt*gyro_unbiased;\n            \n            Eigen::Quaterniond q_hat_dt2 = q_hat*Eigen::AngleAxisd(delta_angle.norm()/2, delta_angle.normalized());\n            \n            Eigen::Quaterniond q_hat_dt = q_hat*Eigen::AngleAxisd(delta_angle.norm(), delta_angle.normalized());\n            \n            // k1 = f(tk, xk)\n            Eigen::Vector3d k1_v = R_hat*acc_unbiased + _gravity;\n            Eigen::Vector3d k1_p = v_hat;\n            \n            // k2 = f(tk+dt/2, xk+k1*dt/2)\n            Eigen::Vector3d k2_v = q_hat_dt2.toRotationMatrix()*acc_unbiased + _gravity;\n            Eigen::Vector3d k2_p = v_hat + k1_v*dt/2.0;\n            \n            // k3 = f(tk+dt/2, xk+k2*dt/2)\n            Eigen::Vector3d k3_v = q_hat_dt2.toRotationMatrix()*acc_unbiased + _gravity;\n            Eigen::Vector3d k3_p = v_hat + k2_v*dt/2.0;\n            \n            // k4 = f(tk+dt, xk+k3*dt)\n            Eigen::Vector3d k4_v = q_hat_dt.toRotationMatrix()*acc_unbiased + _gravity;\n            Eigen::Vector3d k4_p = v_hat + k3_v*dt;\n            \n            // update p and v nominal states using RK4\n            Eigen::Vector3d v_hat_new = v_hat + dt/6.0*(k1_v + 2.0*k2_v + 2.0*k3_v + k4_v);\n            Eigen::Vector3d p_hat_new = p_hat + dt/6.0*(k1_p + 2.0*k2_p + 2.0*k3_p + k4_p);\n            \n            state->_extended_pose->setValueLinearByQuat(q_hat_dt);\n            \n            state->_extended_pose->setValueTrans1(p_hat_new);\n            \n            state->_extended_pose->setValueTrans2(v_hat_new);\n            \n            if (state->_state_params._enable_gnss)\n                for (int i = 0; i < 4; ++i)\n                    if (state->_gnss.find(static_cast<State::GNSSType>(i)) != state->_gnss.end() && state->_gnss.find(State::GNSSType::FS) != state->_gnss.end())\n                    {\n                        double clockbias_hat = state->_gnss.at(static_cast<State::GNSSType>(i))->value();\n                        \n                        double clockbias_hat_new = clockbias_hat + dt*state->_gnss.at(State::GNSSType::FS)->value();\n                        \n                        state->_gnss.at(static_cast<State::GNSSType>(i))->setValue(clockbias_hat_new);\n                    }\n            \n            Eigen::Matrix<double, 15, 15> F = Eigen::Matrix<double, 15, 15>::Zero();\n            \n            F.block<3, 3>(3, 6) = Eigen::Matrix3d::Identity();\n            F.block<3, 3>(6, 0) = skew(_gravity);\n            \n            F.block<3, 3>(0, 9) = -R_hat;\n            F.block<3, 3>(3, 9) = -skew(p_hat)*R_hat;\n            F.block<3, 3>(6, 9) = -skew(v_hat)*R_hat;\n            F.block<3, 3>(6, 12) = -R_hat;\n            \n            Eigen::Matrix<double, 15, 15> F2 = F*F/2.0;\n            Eigen::Matrix<double, 15, 15> F3 = F2*F/3.0;\n            \n            Phi = Eigen::Matrix<double, 15, 15>::Identity() + F*dt + F2*dt*dt + F3*std::pow(dt, 3.0);\n        }\n    }\n    \n    void ImuPropagator::propagateUntil(std::shared_ptr<State> state,\n                                       double t_end,\n                                       bool isAnalytic)\n    {\n        if (!_has_gravity_set || t_end <= state->_timestamp) return;\n        \n        if (_imu_ctrl_buffer.size() == 0) return;\n        \n        if (_imu_ctrl_buffer[0]._timestamp > t_end) return;\n        \n        int propa_cnt = 0;\n        \n        ImuCtrl last_imu_ctrl = _imu_ctrl_buffer[_imu_ctrl_buffer.size()-1];\n        \n        for (int i = 0; i < _imu_ctrl_buffer.size(); ++i)\n        {\n            double ctrl_time = _imu_ctrl_buffer[i]._timestamp;\n            \n            if (ctrl_time < state->_timestamp)\n            {\n                ++propa_cnt;\n                continue;\n            }\n            \n            if (ctrl_time > t_end) break;\n            \n            ++propa_cnt;\n            \n            double dt = ctrl_time - state->_timestamp;\n                    \n            if (dt < 1e-6) continue;\n            \n            Eigen::Matrix<double, 15, 15> Phi;\n            Eigen::Matrix<double, 15, 12> G;\n            \n            last_imu_ctrl = _imu_ctrl_buffer[i];\n            \n            this->stateAndCovTransition(state, _imu_ctrl_buffer[i], dt, Phi, G, isAnalytic);\n            \n            StateManager::propagateStateCov(state, Phi, G, dt);\n        }\n        \n        if (state->_timestamp < t_end)\n        {\n            double dt_last = t_end - state->_timestamp;\n            \n            if (dt_last > 1e-06)\n            {\n                Eigen::Matrix<double, 15, 15> Phi;\n                Eigen::Matrix<double, 15, 12> G;\n                \n                this->stateAndCovTransition(state, last_imu_ctrl, dt_last, Phi, G, isAnalytic);\n                \n                StateManager::propagateStateCov(state, Phi, G, dt_last);\n            }\n            else\n                state->_timestamp = t_end;\n        }\n        \n        _imu_ctrl_buffer.erase(_imu_ctrl_buffer.begin(), _imu_ctrl_buffer.begin()+propa_cnt);\n    }\n    \n    void ImuPropagator::propagateAugmentAtEnd(std::shared_ptr<State> state,\n                                              double t_end,\n                                              bool isAnalytic)\n    {\n        if (!_has_gravity_set) return;\n        \n        this->propagateUntil(state, t_end, isAnalytic);\n        \n        if (state->_timestamp < t_end)\n        {\n            std::cout << \"[ImuPropagator]: Cannot propa to t_end due to no imu ctrl!\" << std::endl;\n            return;\n        }\n        else if (state->_timestamp > t_end)\n        {\n            std::cout << \"[IMUPropagator]: Cannot propa because t_end < curr state time!\" << std::endl;\n            return;\n        }\n        \n        StateManager::augmentSlidingWindowPose(state);\n    }\n    \n    void ImuPropagator::propagateToExpectedPoseAndAugment(std::shared_ptr<State> state,\n                                                          double t_end,\n                                                          const Eigen::Isometry3d& T_i2w)\n    {\n        if (!_has_gravity_set) return;\n        \n        state->_extended_pose->setValueLinearByMat(T_i2w.linear());\n        state->_extended_pose->setValueTrans1(T_i2w.translation());\n        state->_extended_pose->setValueTrans2(Eigen::Vector3d::Zero());\n        \n        state->_bg->setIdentity();\n        state->_ba->setIdentity();\n        \n        state->_camleft_imu_extrinsics->setIdentity();\n        \n        state->_timestamp = t_end;\n        \n        StateManager::augmentSlidingWindowPose(state);\n    }\n}\n"
  },
  {
    "path": "ingvio_estimator/src/ImuPropagator.h",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#pragma once\n\n#include <memory>\n#include <iostream>\n\n#include <Eigen/Core>\n#include <Eigen/Geometry>\n\n#include <vector>\n\n#include <sensor_msgs/Imu.h>\n\n#include <eigen_conversions/eigen_msg.h>\n\n#include \"IngvioParams.h\"\n#include \"AuxGammaFunc.h\"\n\nnamespace ingvio\n{\n    class State;\n    \n    class ImuCtrl\n    {\n    public:\n        ImuCtrl() : _timestamp(-1), _accel_raw(Eigen::Vector3d::Zero()), _gyro_raw(Eigen::Vector3d::Zero()) {}\n        \n        ImuCtrl(const sensor_msgs::Imu::ConstPtr& imu_msg)\n        {\n            _timestamp = imu_msg->header.stamp.toSec();\n            \n            tf::vectorMsgToEigen(imu_msg->angular_velocity, _gyro_raw);\n            tf::vectorMsgToEigen(imu_msg->linear_acceleration, _accel_raw);\n        }\n        \n        double _timestamp;\n        \n        Eigen::Vector3d _accel_raw;\n        Eigen::Vector3d _gyro_raw;\n        \n        ImuCtrl operator+(const ImuCtrl& other_imu_ctrl)\n        {\n            ImuCtrl result;\n            \n            result._accel_raw = this->_accel_raw + other_imu_ctrl._accel_raw;\n            result._gyro_raw = this->_gyro_raw + other_imu_ctrl._gyro_raw;\n            \n            return result;\n        }\n        \n        ImuCtrl& operator=(const ImuCtrl& other_imu_ctrl)\n        {\n            this->_timestamp = other_imu_ctrl._timestamp;\n            this->_accel_raw = other_imu_ctrl._accel_raw;\n            this->_gyro_raw = other_imu_ctrl._gyro_raw;\n            \n            return *this;\n        }\n        \n        void setRandom()\n        {\n            _accel_raw.setRandom();\n            _gyro_raw.setRandom();\n        }\n    };\n    \n    class ImuPropagator\n    {\n    public:\n        ImuPropagator() : _has_gravity_set(true) \n        {\n            _init_imu_buffer_sp = -1;\n            _init_gravity = 9.8;\n            \n            _gravity = Eigen::Vector3d(0.0, 0.0, -9.8);\n            _quat_init.setIdentity();\n        }\n        \n        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)\n        {\n            if (_init_imu_buffer_sp < 0)\n            {\n                _has_gravity_set = true;\n                \n                _gravity = Eigen::Vector3d(0.0, 0.0, -_init_gravity);\n                \n                _quat_init.setIdentity();\n            }\n        }\n        \n        void reset(bool isResetGravity = false)\n        {\n            _has_gravity_set = !isResetGravity;\n            _imu_ctrl_buffer.clear();\n            \n            if (_init_imu_buffer_sp < 0)\n            {\n                _has_gravity_set = true;\n                \n                _gravity = Eigen::Vector3d(0.0, 0.0, -_init_gravity);\n                \n                _quat_init.setIdentity();\n            }\n        }\n        \n        void storeImu(const ImuCtrl& imu_ctrl);\n        \n        void storeImu(const sensor_msgs::Imu::ConstPtr& imu_msg)\n        {\n            ImuCtrl tmp(imu_msg);\n            this->storeImu(tmp);\n        }\n        \n        bool getInitQuat(Eigen::Quaterniond& quat_init) const\n        { \n            if (!_has_gravity_set)\n            {\n                quat_init.setIdentity();\n                return false;\n            }\n            else\n            {\n                quat_init = _quat_init;\n                return true;\n            }\n        }\n        \n        bool getAvgQuat(Eigen::Quaterniond& quat_avg, int num_ctrls = 1);\n        \n        const Eigen::Vector3d& getGravity() const\n        {\n            return _gravity;\n        }\n        \n        const bool& isInit() const\n        { return _has_gravity_set; }\n        \n        void stateAndCovTransition(std::shared_ptr<State> state,\n                                   const ImuCtrl& imu_ctrl,\n                                   double dt,\n                                   Eigen::Matrix<double, 15, 15>& Phi,\n                                   Eigen::Matrix<double, 15, 12>& G,\n                                   bool isAnalytic = true);\n        \n        void propagateUntil(std::shared_ptr<State> state,\n                            double t_end,\n                            bool isAnalytic = true);\n        \n        void propagateAugmentAtEnd(std::shared_ptr<State> state,\n                                   double t_end,\n                                   bool isAnalytic = true);\n        \n        void propagateToExpectedPoseAndAugment(std::shared_ptr<State> state,\n                                               double t_end,\n                                               const Eigen::Isometry3d& T_i2w);\n        \n    protected:\n        \n        bool _has_gravity_set; \n        \n        double _init_gravity;\n        int _max_imu_buffer_size;\n        int _init_imu_buffer_sp;\n        \n        Eigen::Vector3d _gravity;\n        \n        Eigen::Quaterniond _quat_init;\n        \n        std::vector<ImuCtrl> _imu_ctrl_buffer;\n    };\n}\n"
  },
  {
    "path": "ingvio_estimator/src/IngvioFilter.cpp",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#include <nav_msgs/Odometry.h>\n#include <tf_conversions/tf_eigen.h>\n\n#include \"IngvioFilter.h\"\n\n#include \"State.h\"\n#include \"StateManager.h\"\n\n#include \"ImuPropagator.h\"\n\n#include \"Triangulator.h\"\n\n#include \"MapServerManager.h\"\n\n#include \"RemoveLostUpdate.h\"\n#include \"SwMargUpdate.h\"\n#include \"KeyframeUpdate.h\"\n#include \"LandmarkUpdate.h\"\n\n#include \"TicToc.h\"\n#include \"Color.h\"\n\n#include \"GnssData.h\"\n#include \"GnssSync.h\"\n#include \"GvioAligner.h\"\n#include \"GnssUpdate.h\"\n\nnamespace ingvio\n{\n    void IngvioFilter::initIO()\n    {\n        _filter_params.readParams(_nh);\n        \n        if (_filter_params._cam_nums == 2)\n        {\n            _sub_stereo_frame = _nh.subscribe(_filter_params._feature_topic, 100, &IngvioFilter::callbackStereoFrame, this);\n        }\n        else if (_filter_params._cam_nums == 1)\n        {\n            _sub_mono_frame = _nh.subscribe(_filter_params._feature_topic, 100, &IngvioFilter::callbackMonoFrame, this);\n        }\n        else\n        {\n            std::cout << \"[IngvioParams]: Cam num \" << _filter_params._cam_nums << \" not supported! Init as mono config!\" << std::endl;\n            \n            _filter_params._cam_nums = 1;\n            \n            _sub_mono_frame = _nh.subscribe(_filter_params._feature_topic, 100, &IngvioFilter::callbackMonoFrame, this);\n        }\n        \n        _sub_imu = _nh.subscribe(_filter_params._imu_topic, 500, &IngvioFilter::callbackIMU, this);\n        \n        _odom_w_pub = _nh.advertise<nav_msgs::Odometry>(\"pose_w\", 5);\n        _path_w_pub = _nh.advertise<nav_msgs::Path>(\"path_w\", 1);\n        \n        _state = std::make_shared<State>(_filter_params);\n        \n        _imu_propa = std::make_shared<ImuPropagator>(_filter_params);\n        \n        _tri = std::make_shared<Triangulator>(_filter_params);\n        \n        _map_server = std::make_shared<MapServer>();\n        \n        _remove_lost_update = std::make_shared<RemoveLostUpdate>(_filter_params);\n        \n        _keyframe_update = std::make_shared<KeyframeUpdate>(_filter_params);\n        \n        _sw_marg_update = std::make_shared<SwMargUpdate>(_filter_params);\n        \n        _landmark_update = std::make_shared<LandmarkUpdate>(_filter_params);\n        \n        _gnss_data = std::make_shared<GnssData>();\n        \n        _gnss_sync = std::make_shared<GnssSync>(_filter_params);\n        \n        _gvio_aligner = std::make_shared<GvioAligner>(_filter_params);\n        \n        _gnss_update = std::make_shared<GnssUpdate>(_filter_params);\n        \n        if (_filter_params._enable_gnss)\n        {\n            _sub_ephem = _nh.subscribe(_filter_params._gnss_ephem_topic, 100, &IngvioFilter::callbackEphem, this);\n            \n            _sub_glo_ephem = _nh.subscribe(_filter_params._gnss_glo_ephem_topic, 100, &IngvioFilter::callbackGloEphem, this);\n            \n            _sub_gnss_meas = _nh.subscribe(_filter_params._gnss_meas_topic, 100, &IngvioFilter::callbackGnssMeas, this);\n            \n            _sub_iono_params = _nh.subscribe(_filter_params._gnss_iono_params_topic, 100, &IngvioFilter::callbackIonoParams, this);\n            \n            _sub_rtk_gt = _nh.subscribe(_filter_params._rtk_gt_topic, 50, &IngvioFilter::callbackRtkGroundTruth, this);\n            \n            _odom_spp_pub = _nh.advertise<nav_msgs::Odometry>(\"pose_spp\", 5);\n            \n            _path_spp_pub = _nh.advertise<nav_msgs::Path>(\"path_spp\", 1);\n      \n            _odom_gt_pub = _nh.advertise<nav_msgs::Odometry>(\"pose_gt\", 5);\n            \n            _path_gt_pub = _nh.advertise<nav_msgs::Path>(\"path_gt\", 1);\n        }\n        \n        _filter_params.printParams();\n    }\n    \n    void IngvioFilter::callbackMonoFrame(const feature_tracker::MonoFrameConstPtr& mono_frame_ptr)\n    {\n        if (_filter_params._enable_gnss && !_gnss_sync->isSync())\n            return;\n        \n        if (!_hasImageCome)\n        {\n            _hasImageCome = true;\n            return;\n        }\n        \n        if (!_hasInitState) return;\n        \n        const double target_time = mono_frame_ptr->header.stamp.toSec();\n        \n        if (_state->_timestamp >= target_time) return;\n    \n        TicToc timer_mono;\n        \n        _imu_propa->propagateAugmentAtEnd(_state, target_time);\n        \n        if (_state->_timestamp < target_time) return;\n        \n        MapServerManager::collectMonoMeas(_map_server, _state, mono_frame_ptr);\n        \n        _remove_lost_update->updateStateMono(_state, _map_server, _tri);\n        \n        if (_filter_params._is_key_frame)\n        {\n            _keyframe_update->updateStateMono(_state, _map_server, _tri);\n            \n            if (_filter_params._max_lm_feats > 0)\n            {\n                _landmark_update->updateLandmarkMono(_state, _map_server);\n                \n                _landmark_update->initNewLandmarkMono(_state, _map_server, \n                                                      _tri, _filter_params._max_sw_clones);\n            }\n            \n            _keyframe_update->cleanMonoObsAtMargTime(_state, _map_server);\n            \n            _keyframe_update->changeMSCKFAnchor(_state, _map_server);\n            \n            if (_filter_params._max_lm_feats > 0)\n            {\n                std::vector<double> marg_kfs;\n                _keyframe_update->getMargKfs(_state, marg_kfs);\n                \n                _landmark_update->changeLandmarkAnchor(_state, _map_server, marg_kfs);\n            }\n            \n            _keyframe_update->margSwPose(_state);\n        }\n        else\n        {\n            _sw_marg_update->updateStateMono(_state, _map_server, _tri);\n            \n            if (_filter_params._max_lm_feats > 0)\n            {\n                _landmark_update->updateLandmarkMono(_state, _map_server);\n                \n                _landmark_update->initNewLandmarkMono(_state, _map_server, \n                                                      _tri, _filter_params._max_sw_clones);\n            }\n            \n            _sw_marg_update->cleanMonoObsAtMargTime(_state, _map_server);\n            \n            _sw_marg_update->changeMSCKFAnchor(_state, _map_server);\n            \n            if (_filter_params._max_lm_feats > 0)\n                _landmark_update->changeLandmarkAnchor(_state, _map_server);\n            \n            _sw_marg_update->margSwPose(_state);\n        }\n\n        MapServerManager::eraseInvalidFeatures(_map_server, _state);\n        \n        if (_filter_params._enable_gnss)\n        {\n            GnssMeas gnss_meas;\n            SppMeas spp_meas;\n            \n            bool flag = false;\n            \n            if (_gnss_sync->getSppAt(mono_frame_ptr->header, spp_meas))\n            {\n                flag = true;\n                visualizeSpp(mono_frame_ptr->header, spp_meas);\n            }\n            \n            if (_gnss_sync->getGnssMeasAt(mono_frame_ptr->header, gnss_meas))\n            {\n                if (flag && !_gvio_aligner->isAlign())\n                    _gvio_aligner->batchAlign(gnss_meas, _state->_extended_pose, _gnss_data->latest_gnss_iono_params);\n                \n                if (_gvio_aligner->isAlign())\n                {\n                    _gnss_update->checkYofStatus(_state, _gvio_aligner);\n                    \n                    // _gnss_update->removeUntrackedSys(_state, gnss_meas);\n                    \n                    _gnss_update->updateTrackedSys(_state, gnss_meas, _gvio_aligner,\n                                                   _gnss_data->latest_gnss_iono_params);\n                    \n                    if (flag)\n                        _gnss_update->addNewTrackedSys(_state, gnss_meas, spp_meas,\n                                                       _gvio_aligner,\n                                                       _gnss_data->latest_gnss_iono_params);\n                }\n            }\n        }\n        \n        visualize(mono_frame_ptr->header);\n        \n        double time_mono = timer_mono.toc();\n        \n        static double total_time = 0.0;\n        static int total_cnt = 0;\n        \n        ++total_cnt;\n        total_time += time_mono;\n        \n        if (time_mono < 50.0)\n            std::cout << color::setGreen << \"[IngvioFilter]: One loop mono callback: \" << total_time/total_cnt << \" (ms) \" << color::resetColor << std::endl;\n        else\n            std::cout << color::setRed << \"[IngvioFilter]: One loop mono callback: \" << total_time/total_cnt << \" (ms) \" << color::resetColor << std::endl;\n    }\n    \n    void IngvioFilter::callbackStereoFrame(const feature_tracker::StereoFrameConstPtr& stereo_frame_ptr)\n    {\n        if (_filter_params._enable_gnss && !_gnss_sync->isSync())\n            return;\n        \n        if (!_hasImageCome)\n        {\n            _hasImageCome = true;\n            return;\n        }\n        \n        if (!_hasInitState) return;\n        \n        const double target_time = stereo_frame_ptr->header.stamp.toSec();\n        \n        if (_state->_timestamp >= target_time) return;\n        \n        TicToc timer_stereo;\n        \n        _imu_propa->propagateAugmentAtEnd(_state, target_time);\n        \n        if (_state->_timestamp < target_time) return;\n        \n        MapServerManager::collectStereoMeas(_map_server, _state, stereo_frame_ptr);\n        \n        _remove_lost_update->updateStateStereo(_state, _map_server, _tri);\n        \n        if (_filter_params._is_key_frame)\n        {\n            _keyframe_update->updateStateStereo(_state, _map_server, _tri);\n            \n            if (_filter_params._max_lm_feats > 0)\n            {\n                _landmark_update->updateLandmarkStereo(_state, _map_server);\n                \n                _landmark_update->initNewLandmarkStereo(_state, _map_server, \n                                                      _tri, _filter_params._max_sw_clones);\n            }\n            \n            _keyframe_update->cleanStereoObsAtMargTime(_state, _map_server);\n            \n            _keyframe_update->changeMSCKFAnchor(_state, _map_server);\n            \n            if (_filter_params._max_lm_feats > 0)\n            {\n                std::vector<double> marg_kfs;\n                _keyframe_update->getMargKfs(_state, marg_kfs);\n                \n                _landmark_update->changeLandmarkAnchor(_state, _map_server, marg_kfs);\n            }\n            \n            _keyframe_update->margSwPose(_state);\n        }\n        else\n        {\n            _sw_marg_update->updateStateStereo(_state, _map_server, _tri);\n            \n            if (_filter_params._max_lm_feats > 0)\n            {\n                _landmark_update->updateLandmarkStereo(_state, _map_server);\n                \n                _landmark_update->initNewLandmarkStereo(_state, _map_server, \n                                                        _tri, _filter_params._max_sw_clones);\n            }\n            \n            _sw_marg_update->cleanStereoObsAtMargTime(_state, _map_server);\n            \n            _sw_marg_update->changeMSCKFAnchor(_state, _map_server);\n            \n            if (_filter_params._max_lm_feats > 0)\n                _landmark_update->changeLandmarkAnchor(_state, _map_server);\n            \n            _sw_marg_update->margSwPose(_state);\n        }\n                \n        MapServerManager::eraseInvalidFeatures(_map_server, _state);\n        \n        if (_filter_params._enable_gnss)\n        {\n            GnssMeas gnss_meas;\n            SppMeas spp_meas;\n            \n            bool flag = false;\n            \n            if (_gnss_sync->getSppAt(stereo_frame_ptr->header, spp_meas))\n            {\n                flag = true;\n                visualizeSpp(stereo_frame_ptr->header, spp_meas);\n            }\n            \n            if (_gnss_sync->getGnssMeasAt(stereo_frame_ptr->header, gnss_meas))\n            {\n                if (flag && !_gvio_aligner->isAlign())\n                    _gvio_aligner->batchAlign(gnss_meas, _state->_extended_pose, _gnss_data->latest_gnss_iono_params);\n                \n                if (_gvio_aligner->isAlign())\n                {\n                    _gnss_update->checkYofStatus(_state, _gvio_aligner);\n                    \n                    // _gnss_update->removeUntrackedSys(_state, gnss_meas);\n                    \n                    _gnss_update->updateTrackedSys(_state, gnss_meas, _gvio_aligner,\n                                                   _gnss_data->latest_gnss_iono_params);\n                    \n                    if (flag)\n                        _gnss_update->addNewTrackedSys(_state, gnss_meas, spp_meas,\n                                                       _gvio_aligner,\n                                                       _gnss_data->latest_gnss_iono_params);\n                }\n            }\n        }\n        \n        visualize(stereo_frame_ptr->header);\n        \n        double time_stereo = timer_stereo.toc();\n        \n        static double total_time = 0.0;\n        static int total_cnt = 0;\n        \n        ++total_cnt;\n        total_time += time_stereo;\n        \n        if (time_stereo < 50.0)\n            std::cout << color::setGreen << \"[IngvioFilter]: One loop stereo callback: \" << total_time/total_cnt << \" (ms) \" << color::resetColor << std::endl;\n        else\n            std::cout << color::setRed << \"[IngvioFilter]: One loop stereo callback: \" << total_time/total_cnt << \" (ms) \" << color::resetColor << std::endl;\n        \n    }\n    \n    void IngvioFilter::callbackIMU(sensor_msgs::Imu::ConstPtr imu_msg)\n    {\n        double aux_time = ros::Time::now().toSec();\n        \n        if (_filter_params._enable_gnss)\n        {\n            _gnss_sync->storeTimePair(aux_time, imu_msg->header.stamp.toSec());\n            \n            if (!_gnss_sync->isSync())\n                return;\n        }\n        \n        if (!_hasImageCome) return;\n        \n        _imu_propa->storeImu(imu_msg);\n        \n        if (_imu_propa->isInit() && !_hasInitState)\n        {\n            Eigen::Quaterniond init_quat;\n            \n            if (_imu_propa->getInitQuat(init_quat))\n            {\n                _state->initStateAndCov(imu_msg->header.stamp.toSec(), init_quat);\n                _hasInitState = true;\n            }\n        }\n    }\n    \n    void IngvioFilter::visualize(const std_msgs::Header& header)\n    {\n        ros::Time time = header.stamp;\n        \n        Eigen::Isometry3d T_i2w = Eigen::Isometry3d::Identity();\n        T_i2w.linear() = _state->_extended_pose->valueLinearAsMat();\n        T_i2w.translation() = _state->_extended_pose->valueTrans1();\n        \n        Eigen::Vector3d vel = _state->_extended_pose->valueTrans2();\n        \n        if (T_i2w.linear().hasNaN() || T_i2w.translation().hasNaN() || vel.hasNaN())\n            return;\n        \n        tf::Transform T_i2w_tf;\n        tf::transformEigenToTF(T_i2w, T_i2w_tf);\n        _tf_pub.sendTransform(tf::StampedTransform(T_i2w_tf, time, \"world\", \"uav\"));\n        \n        nav_msgs::Odometry odom_w_msg;\n        odom_w_msg.header.stamp = time;\n        odom_w_msg.header.frame_id = \"world\";\n        odom_w_msg.child_frame_id = \"uav\";\n        \n        tf::poseEigenToMsg(T_i2w, odom_w_msg.pose.pose);\n        tf::vectorEigenToMsg(vel, odom_w_msg.twist.twist.linear);\n        \n        // ROS_INFO(\"Velocity vx = %f vy = %f vz = %f\", vel(0), vel(1), vel(2));\n        // 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);\n        \n        _odom_w_pub.publish(odom_w_msg);\n        \n        geometry_msgs::PoseStamped pose_w;\n        pose_w.header.stamp = time;\n        pose_w.header.frame_id = \"world\";\n        pose_w.pose.position.x = odom_w_msg.pose.pose.position.x;\n        pose_w.pose.position.y = odom_w_msg.pose.pose.position.y;\n        pose_w.pose.position.z = odom_w_msg.pose.pose.position.z;\n        pose_w.pose.orientation.x = odom_w_msg.pose.pose.orientation.x;\n        pose_w.pose.orientation.y = odom_w_msg.pose.pose.orientation.y;\n        pose_w.pose.orientation.z = odom_w_msg.pose.pose.orientation.z;\n        pose_w.pose.orientation.w = odom_w_msg.pose.pose.orientation.w;\n        \n        _path_w_msg.header.stamp = time;\n        _path_w_msg.header.frame_id = \"world\";\n        _path_w_msg.poses.push_back(pose_w);\n        _path_w_pub.publish(_path_w_msg);\n    }\n    \n    void IngvioFilter::visualizeSpp(const std_msgs::Header& header, const SppMeas& spp_meas)\n    {\n        if (!_filter_params._enable_gnss || !_gnss_sync->isSync() || !_gvio_aligner->isAlign())\n            return;\n        \n        Eigen::Vector3d pos_spp_w = _gvio_aligner->getTecef2w()*spp_meas.posSpp.block<3, 1>(0, 0);\n        \n        if (pos_spp_w.hasNaN())\n            return;\n    \n        geometry_msgs::PoseStamped pose_spp;\n        pose_spp.header.stamp = header.stamp;\n        pose_spp.header.frame_id = \"world\";\n        pose_spp.pose.position.x = pos_spp_w.x();\n        pose_spp.pose.position.y = pos_spp_w.y();\n        pose_spp.pose.position.z = pos_spp_w.z();\n        pose_spp.pose.orientation.x = 0.0;\n        pose_spp.pose.orientation.y = 0.0;\n        pose_spp.pose.orientation.z = 0.0;\n        pose_spp.pose.orientation.w = 1.0;\n        \n        _path_spp_msg.header.stamp = pose_spp.header.stamp;\n        _path_spp_msg.header.frame_id = \"world\";\n        _path_spp_msg.poses.push_back(pose_spp);\n        _path_spp_pub.publish(_path_spp_msg);\n        \n        Eigen::Isometry3d T_i2w = Eigen::Isometry3d::Identity();\n        T_i2w.translation() = pos_spp_w;\n        \n        Eigen::Vector3d vel_spp_w = _gvio_aligner->getRecef2enu()*spp_meas.velSpp.block<3, 1>(0, 0);\n        if (vel_spp_w.hasNaN())\n            return;\n        \n        nav_msgs::Odometry odom_spp_msg;\n        odom_spp_msg.header.stamp = header.stamp;\n        odom_spp_msg.header.frame_id = \"world\";\n        odom_spp_msg.child_frame_id = \"spp\";\n        \n        tf::poseEigenToMsg(T_i2w, odom_spp_msg.pose.pose);\n        tf::vectorEigenToMsg(vel_spp_w, odom_spp_msg.twist.twist.linear);\n        \n        _odom_spp_pub.publish(odom_spp_msg);\n    }\n}\n"
  },
  {
    "path": "ingvio_estimator/src/IngvioFilter.h",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#pragma once\n\n#include <memory>\n\n#include <ros/ros.h>\n#include <std_msgs/Header.h>\n#include <nav_msgs/Path.h>\n\n#include <sensor_msgs/NavSatFix.h>\n#include <sensor_msgs/Imu.h>\n\n#include <gnss_comm/gnss_ros.hpp>\n\n#include <tf/transform_broadcaster.h>\n\n#include <feature_tracker/MonoFrame.h>\n#include <feature_tracker/StereoFrame.h>\n\n#include \"IngvioParams.h\"\n#include \"MapServer.h\"\n\nnamespace ingvio\n{\n    class State;\n    class ImuPropagator;\n    class Triangulator;\n    class RemoveLostUpdate;\n    class SwMargUpdate;\n    class KeyframeUpdate;\n    class LandmarkUpdate;\n    class GnssData;\n    class GnssSync;\n    class GvioAligner;\n    struct SppMeas;\n    class GnssUpdate;\n    \n    class IngvioFilter\n    {\n    public:\n        IngvioFilter(ros::NodeHandle& n) : _nh(n) {}\n        \n        ~IngvioFilter() = default;\n        \n        IngvioFilter(const IngvioFilter&) = delete;\n        \n        IngvioFilter operator=(const IngvioFilter&) = delete;\n        \n        void initIO();\n        \n    protected:\n        \n        IngvioParams _filter_params;\n        \n        ros::NodeHandle _nh;\n        \n        ros::Subscriber _sub_mono_frame, _sub_stereo_frame, _sub_imu;\n        ros::Publisher _odom_w_pub, _path_w_pub;\n        tf::TransformBroadcaster _tf_pub;\n        \n        ros::Subscriber _sub_ephem, _sub_glo_ephem, _sub_gnss_meas, _sub_iono_params, _sub_rtk_gt;\n        \n        ros::Publisher _odom_spp_pub, _odom_gt_pub;\n        ros::Publisher _path_spp_pub, _path_gt_pub;\n        \n        nav_msgs::Path _path_w_msg, _path_spp_msg, _path_gt_msg;\n        \n        bool _hasImageCome = false;\n        \n        bool _hasInitState = false;\n        \n        std::shared_ptr<State> _state;\n        \n        std::shared_ptr<ImuPropagator> _imu_propa;\n        \n        std::shared_ptr<Triangulator> _tri;\n        \n        std::shared_ptr<MapServer> _map_server;\n        \n        std::shared_ptr<RemoveLostUpdate> _remove_lost_update;\n        \n        std::shared_ptr<SwMargUpdate> _sw_marg_update;\n        \n        std::shared_ptr<KeyframeUpdate> _keyframe_update;\n        \n        std::shared_ptr<LandmarkUpdate> _landmark_update;\n        \n        std::shared_ptr<GnssData> _gnss_data;\n        \n        std::shared_ptr<GnssSync> _gnss_sync;\n        \n        std::shared_ptr<GvioAligner> _gvio_aligner;\n        \n        std::shared_ptr<GnssUpdate> _gnss_update;\n        \n        void callbackMonoFrame(const feature_tracker::MonoFrameConstPtr& mono_frame_ptr);\n        \n        void callbackStereoFrame(const feature_tracker::StereoFrameConstPtr& stereo_frame_ptr);\n        \n        void callbackIMU(sensor_msgs::Imu::ConstPtr imu_msg);\n        \n        void callbackEphem(const gnss_comm::GnssEphemMsgConstPtr& ephem_msg);\n        \n        void callbackGloEphem(const gnss_comm::GnssGloEphemMsgConstPtr& glo_ephem_msg);\n        \n        void callbackIonoParams(const gnss_comm::StampedFloat64ArrayConstPtr& iono_msg);\n        \n        void callbackGnssMeas(const gnss_comm::GnssMeasMsgConstPtr& gnss_meas_msg);\n        \n        void callbackRtkGroundTruth(const sensor_msgs::NavSatFix::ConstPtr& nav_sat_msg);\n        \n        void visualize(const std_msgs::Header& header);\n        \n        void visualizeSpp(const std_msgs::Header& header, const SppMeas& spp_meas);\n    };\n    \n    typedef std::shared_ptr<IngvioFilter> IngvioFilterPtr;\n}\n"
  },
  {
    "path": "ingvio_estimator/src/IngvioNode.cpp",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#include \"IngvioFilter.h\"\n\nusing namespace ingvio;\n\nint main(int argc, char** argv)\n{\n    ros::init(argc, argv, \"ingvio_estimator\");\n    ros::NodeHandle n(\"~\");\n    \n    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);\n    \n    IngvioFilterPtr ingvio_filter_ptr = std::make_shared<IngvioFilter>(n);\n    \n    ingvio_filter_ptr->initIO();\n    \n    ros::spin();\n    \n    return 0;\n}\n"
  },
  {
    "path": "ingvio_estimator/src/IngvioParams.cpp",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#include \"IngvioParams.h\"\n\n#include <opencv2/core/eigen.hpp>\n\nnamespace ingvio\n{\n    void IngvioParams::readParams(ros::NodeHandle& nh)\n    {\n        _config_path = readParams<std::string>(nh, \"config_file\");\n        this->readParams(_config_path);\n    }\n    \n    void IngvioParams::readParams(const std::string& config_path)\n    {\n        cv::FileStorage fs(config_path, cv::FileStorage::READ);\n        \n        if (!fs.isOpened())\n            ROS_ERROR(\"[IngvioParams]: Wrong path to config file!\");\n        \n        _cam_nums = fs[\"cam_nums\"];\n        if (_cam_nums >= 3)\n        {\n            ROS_WARN(\"[IngvioParams]: Camera number not available! Switch to mono!\");\n            _cam_nums = 1;\n        }\n        \n        fs[\"cam_left_file_path\"] >> _cam_left_config_path;\n        \n        if (_cam_nums == 2)\n            fs[\"cam_right_file_path\"] >> _cam_right_config_path;\n        \n        fs[\"feature_topic\"] >> _feature_topic;\n        fs[\"imu_topic\"] >> _imu_topic;\n        \n        _max_sw_clones = fs[\"max_sliding_window_poses\"];\n        \n        _max_lm_feats = fs[\"max_landmark_features\"];\n        _is_key_frame = fs[\"is_key_frame\"];\n        _enable_gnss = fs[\"enable_gnss\"];\n        \n        _noise_g = fs[\"noise_gyro\"];\n        _noise_a = fs[\"noise_accel\"];\n        _noise_bg = fs[\"noise_bias_gyro\"];\n        _noise_ba = fs[\"noise_bias_accel\"];\n        \n        _init_cov_rot = fs[\"init_cov_rot\"];\n        _init_cov_pos = fs[\"init_cov_pos\"];\n        _init_cov_vel = fs[\"init_cov_vel\"];\n        _init_cov_bg = fs[\"init_cov_bg\"];\n        _init_cov_ba = fs[\"init_cov_ba\"];\n        _init_cov_ext_rot = fs[\"init_cov_ext_rot\"];\n        _init_cov_ext_pos = fs[\"init_cov_ext_pos\"];\n    \n        _init_gravity = fs[\"gravity_norm\"];\n        _max_imu_buffer_size = fs[\"max_imu_buffer_size\"];\n        _init_imu_buffer_sp = fs[\"init_imu_buffer_sp\"];\n        \n        _trans_thres = fs[\"trans_thres\"];\n        _huber_epsilon = fs[\"huber_epsilon\"];\n        _conv_precision = fs[\"conv_precision\"];\n        _init_damping = fs[\"init_damping\"];\n        _outer_loop_max_iter = fs[\"outer_loop_max_iter\"];\n        _inner_loop_max_iter = fs[\"inner_loop_max_iter\"];\n        _max_depth = fs[\"max_depth\"];\n        _min_depth = fs[\"min_depth\"];\n        _max_baseline_ratio = fs[\"max_baseline_ratio\"];\n        \n        _chi2_max_dof = fs[\"chi2_max_dof\"];\n        _chi2_thres = fs[\"chi2_thres\"];\n        _visual_noise = fs[\"visual_noise\"];\n        _frame_select_interval = fs[\"frame_select_interval\"];\n        \n        if (_enable_gnss)\n        {\n            _noise_clockbias = fs[\"noise_rcv_clockbias\"];\n            _noise_cb_rw = fs[\"noise_rcv_clockbias_randomwalk\"];\n            \n            _init_cov_rcv_clockbias = fs[\"init_cov_rcv_clockbias\"];\n            _init_cov_rcv_clockbias_randomwalk = fs[\"init_cov_rcv_clockbias_randomwalk\"];\n            _init_cov_yof = fs[\"init_cov_yof\"];\n            \n            fs[\"gnss_ephem_topic\"] >> _gnss_ephem_topic;\n            fs[\"gnss_glo_ephem_topic\"] >> _gnss_glo_ephem_topic;\n            fs[\"gnss_meas_topic\"] >> _gnss_meas_topic;\n            fs[\"gnss_iono_params_topic\"] >> _gnss_iono_params_topic;\n            fs[\"rtk_gt_topic\"] >> _rtk_gt_topic;\n            \n            _gnss_elevation_thres = fs[\"gnss_elevation_thres\"];\n            _gnss_psr_std_thres = fs[\"gnss_psr_std_thres\"];\n            _gnss_dopp_std_thres = fs[\"gnss_dopp_std_thres\"];\n            _gnss_track_num_thres = fs[\"gnss_track_num_thres\"];\n            \n            _use_fix_time_offset = fs[\"use_fix_time_offset\"];\n            \n            if (_use_fix_time_offset)\n                _gnss_local_offset = fs[\"gnss_local_offset\"];\n            \n            _is_gnss_chi2_test = fs[\"gnss_chi2_test\"];\n            _is_gnss_strong_reject = fs[\"gnss_strong_reject\"];\n            \n            _gv_align_batch_size = fs[\"gv_align_batch_size\"];\n            _gv_align_max_iter = fs[\"gv_align_max_iter\"];\n            _gv_align_conv_epsilon = fs[\"gv_align_conv_epsilon\"];\n            _gv_align_vel_thres = fs[\"gv_align_vel_thres\"];\n            \n            _psr_noise_amp = fs[\"psr_noise_amp\"];\n            _dopp_noise_amp = fs[\"dopp_noise_amp\"];\n            _is_adjust_yof = fs[\"is_adjust_yof\"];\n        }\n        \n        \n        fs.release();\n        \n        cv::FileStorage fs_cam_left(_cam_left_config_path, cv::FileStorage::READ);\n        if (!fs_cam_left.isOpened())\n            ROS_ERROR(\"[IngvioParams]: Wrong path to left cam config file!\");\n        \n        cv::Mat cv_R1, cv_T1;\n        \n        fs_cam_left[\"extrinsicRotation\"] >> cv_R1;\n        fs_cam_left[\"extrinsicTranslation\"] >> cv_T1;\n        Eigen::Matrix3d eigen_R1;\n        Eigen::Vector3d eigen_T1;\n        cv::cv2eigen(cv_R1, eigen_R1);\n        cv::cv2eigen(cv_T1, eigen_T1);\n        \n        _T_cl2i.linear() = eigen_R1;\n        _T_cl2i.translation() = eigen_T1;\n        \n        fs_cam_left.release();\n        \n        if (_cam_nums == 2)\n        {\n            cv::FileStorage fs_cam_right(_cam_right_config_path, cv::FileStorage::READ);\n            \n            if (!fs_cam_right.isOpened())\n                ROS_ERROR(\"[IngvioParams]: Wrong path to right cam config file!\");\n            \n            cv::Mat cv_R2, cv_T2;\n            \n            fs_cam_right[\"extrinsicRotation\"] >> cv_R2;\n            fs_cam_right[\"extrinsicTranslation\"] >> cv_T2;\n            Eigen::Matrix3d eigen_R2;\n            Eigen::Vector3d eigen_T2;\n            cv::cv2eigen(cv_R2, eigen_R2);\n            cv::cv2eigen(cv_T2, eigen_T2);\n            \n            _T_cr2i.linear() = eigen_R2;\n            _T_cr2i.translation() = eigen_T2;\n            \n            fs_cam_right.release();\n        }\n        \n    }\n    \n    void IngvioParams::printParams()\n    {\n        std::cout << \"===== Ingvio Parameter List =====\" << std::endl;\n        \n        std::cout << \"Number of CAMS: \" << _cam_nums << std::endl;\n        \n        std::cout << \"InEKF_config_path: \" << _config_path << std::endl;\n        \n        std::cout << \"cam_left_file_path: \" << _cam_left_config_path << std::endl;\n        \n        if (_cam_nums == 2)\n            std::cout << \"cam_right_file_path: \" << _cam_right_config_path << std::endl;\n        \n        std::cout << \"feature_topic: \" << _feature_topic << std::endl;\n        \n        std::cout << \"imu_topic: \" << _imu_topic << std::endl;\n        \n        std::cout << \"max_sliding_window_poses: \" << _max_sw_clones << std::endl;\n        \n        std::cout << \"is_key_frame: \" << _is_key_frame << std::endl;\n        \n        std::cout << \"max_landmark_features: \" << _max_lm_feats << std::endl;\n        \n        std::cout << \"enable_gnss: \" << _enable_gnss << std::endl;\n        \n        std::cout << \"noise_gyro: \" << _noise_g << std::endl;\n        \n        std::cout << \"noise_accel: \" << _noise_a << std::endl;\n        \n        std::cout << \"noise_bias_gyro: \" << _noise_bg << std::endl;\n        \n        std::cout << \"noise_bias_accel: \" << _noise_ba << std::endl;\n        \n        if (_enable_gnss)\n        {\n            std::cout << \"noise_rcv_clockbias: \" << _noise_clockbias << std::endl;\n            std::cout << \"noise_rcv_clockbias_randomwalk: \" << _noise_cb_rw << std::endl;\n        }\n        \n        std::cout << \"init_cov_rot: \" << _init_cov_rot << std::endl;\n        \n        std::cout << \"init_cov_pos: \" << _init_cov_pos << std::endl;\n        \n        std::cout << \"init_cov_vel: \" << _init_cov_vel << std::endl;\n        \n        std::cout << \"init_cov_bg: \" << _init_cov_bg << std::endl;\n        \n        std::cout << \"init_cov_ba: \" << _init_cov_ba << std::endl;\n        \n        std::cout << \"init_cov_ext_rot: \" << _init_cov_ext_rot << std::endl;\n        \n        std::cout << \"init_cov_ext_pos: \" << _init_cov_ext_pos << std::endl;\n        \n        std::cout << \"gravity_norm: \" << _init_gravity << std::endl;\n\n        std::cout << \"max_imu_buffer_size: \" << _max_imu_buffer_size << std::endl;\n        \n        std::cout << \"init_imu_buffer_sp: \" << _init_imu_buffer_sp << std::endl;\n\n        std::cout << \"trans_thres: \" << _trans_thres << std::endl;\n        \n        std::cout << \"huber_epsilon: \" << _huber_epsilon << std::endl;\n        \n        std::cout << \"conv_precision: \" << _conv_precision << std::endl;\n        \n        std::cout << \"init_damping: \" << _init_damping << std::endl;\n        \n        std::cout << \"outer_loop_max_iter: \" << _outer_loop_max_iter << std::endl;\n        \n        std::cout << \"inner_loop_max_iter: \" << _inner_loop_max_iter << std::endl;\n        \n        std::cout << \"max depth: \" << _max_depth << std::endl;\n        \n        std::cout << \"min_depth: \" << _min_depth << std::endl;\n        \n        std::cout << \"max_baseline_ratio: \" << _max_baseline_ratio << std::endl;\n\n        std::cout << \"chi2_max_dof: \" << _chi2_max_dof << std::endl;\n         \n        std::cout << \"chi2_thres: \" << _chi2_thres << std::endl;\n        \n        std::cout << \"visual_noise: \" << _visual_noise << std::endl;\n        \n        std::cout << \"frame_select_interval: \" <<  _frame_select_interval << std::endl;\n        \n        if (_enable_gnss)\n        {\n            std::cout << \"init_cov_rcv_clockbias: \" << _init_cov_rcv_clockbias << std::endl;\n            \n            std::cout << \"init_cov_rcv_clockbias_randomwalk: \" << _init_cov_rcv_clockbias_randomwalk << std::endl;\n        \n            std::cout << \"init_cov_yof: \" << _init_cov_yof << std::endl;\n            \n            std::cout << \"gnss_ephem_topic: \" << _gnss_ephem_topic << std::endl;\n            \n            std::cout << \"gnss_glo_ephem_topic: \" << _gnss_glo_ephem_topic << std::endl;\n            \n            std::cout << \"gnss_meas_topic: \" << _gnss_meas_topic << std::endl;\n            \n            std::cout << \"gnss_iono_params_topic: \" << _gnss_iono_params_topic << std::endl;\n            \n            std::cout << \"rtk_gt_topic: \" << _rtk_gt_topic << std::endl; \n            \n            std::cout << \"gnss_elevation_thres: \" << _gnss_elevation_thres << std::endl;\n            \n            std::cout << \"gnss_psr_std_thres: \" << _gnss_psr_std_thres << std::endl;\n            \n            std::cout << \"gnss_dopp_std_thres: \" << _gnss_dopp_std_thres << std::endl;\n            \n            std::cout << \"gnss_track_num_thres: \" << _gnss_track_num_thres << std::endl;\n            \n            std::cout << \"use_fix_time_offset: \" << _use_fix_time_offset << std::endl;\n            \n            if (_use_fix_time_offset)\n                std::cout << \"gnss_local_offset: \" << _gnss_local_offset << std::endl;\n            \n            std::cout << \"gnss_chi2_test: \" << _is_gnss_chi2_test << std::endl;\n            \n            std::cout << \"gnss_strong_reject: \" << _is_gnss_strong_reject << std::endl;\n            \n            std::cout << \"gv_align_batch_size: \" << _gv_align_batch_size << std::endl;\n            \n            std::cout << \"gv_align_max_iter: \" << _gv_align_max_iter << std::endl;\n            \n            std::cout << \"gv_align_conv_epsilon: \" << _gv_align_conv_epsilon << std::endl;\n        \n            std::cout << \"gv_align_vel_thres: \" << _gv_align_vel_thres << std::endl;\n            \n            std::cout << \"psr_noise_amp: \" << _psr_noise_amp << std::endl;\n            \n            std::cout << \"dopp_noise_amp: \" << _dopp_noise_amp << std::endl;\n            \n            std::cout << \"is_adjust_yof: \" << _is_adjust_yof << std::endl;\n        }\n        \n        std::cout << \"===============================\" << std::endl;\n        \n    }\n}\n"
  },
  {
    "path": "ingvio_estimator/src/IngvioParams.h",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#pragma once\n\n#include <Eigen/Core>\n#include <Eigen/Geometry>\n#include <string>\n\n#include <ros/ros.h>\n#include <opencv2/highgui/highgui.hpp>\n\nnamespace ingvio\n{\n    class IngvioParams\n    {\n    public:\n        IngvioParams() : _T_cl2i(Eigen::Isometry3d::Identity()), _T_cr2i(Eigen::Isometry3d::Identity()) {}\n        \n        IngvioParams(const IngvioParams&) = delete;\n        \n        IngvioParams operator=(const IngvioParams&) = delete;\n        \n        ~IngvioParams() = default;\n        \n        std::string _config_path;\n        \n        int _cam_nums;\n        std::string _cam_left_config_path;\n        std::string _cam_right_config_path;\n        \n        Eigen::Isometry3d _T_cl2i, _T_cr2i;\n        \n        std::string _feature_topic;\n        std::string _imu_topic;\n        \n        int _max_sw_clones;\n        int _is_key_frame;\n        int _max_lm_feats;\n        int _enable_gnss;\n        \n        double _noise_g;\n        double _noise_a;\n        double _noise_bg;\n        double _noise_ba;\n        double _noise_clockbias;\n        double _noise_cb_rw;\n        \n        double _init_cov_rot;\n        double _init_cov_pos;\n        double _init_cov_vel;\n        double _init_cov_bg;\n        double _init_cov_ba;\n        double _init_cov_ext_rot;\n        double _init_cov_ext_pos;\n        double _init_cov_rcv_clockbias;\n        double _init_cov_rcv_clockbias_randomwalk;\n        double _init_cov_yof;\n        \n        double _init_gravity;\n        int _max_imu_buffer_size;\n        int _init_imu_buffer_sp;\n        \n        double _trans_thres;\n        double _huber_epsilon;\n        double _conv_precision;\n        double _init_damping;\n        int _outer_loop_max_iter;\n        int _inner_loop_max_iter;\n        double _max_depth;\n        double _min_depth;\n        double _max_baseline_ratio;\n        \n        int _chi2_max_dof;\n        double _chi2_thres;\n        double _visual_noise;\n        \n        int _frame_select_interval;\n        \n        std::string _gnss_ephem_topic;\n        std::string _gnss_glo_ephem_topic;\n        std::string _gnss_meas_topic;\n        std::string _gnss_iono_params_topic;\n        std::string _rtk_gt_topic;\n        \n        double _gnss_elevation_thres;\n        double _gnss_psr_std_thres;\n        double _gnss_dopp_std_thres;\n        int _gnss_track_num_thres;\n        \n        int _use_fix_time_offset;\n        double _gnss_local_offset;\n        int _is_gnss_chi2_test;\n        int _is_gnss_strong_reject;\n        \n        int _gv_align_batch_size;\n        int _gv_align_max_iter;\n        double _gv_align_conv_epsilon;\n        double _gv_align_vel_thres;\n        \n        double _psr_noise_amp;\n        double _dopp_noise_amp;\n        int _is_adjust_yof;\n        \n        void readParams(ros::NodeHandle& nh);\n        void readParams(const std::string& config_path);\n        \n        void printParams();\n        \n        template <typename T>\n        static T readParams(ros::NodeHandle& n, std::string name);\n    };\n    \n    template <typename T>\n    T IngvioParams::readParams(ros::NodeHandle& n, std::string name)\n    {\n        T ans;\n        \n        if (n.getParam(name, ans))\n        {\n            ROS_INFO_STREAM(\"Loaded \" << name << \": \" << ans);    \n        }\n        else\n        {\n            ROS_ERROR_STREAM(\"Failed to load: \" << name);\n            n.shutdown();\n        }\n        \n        return ans;\n    }\n}\n"
  },
  {
    "path": "ingvio_estimator/src/KeyframeUpdate.cpp",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#include <Eigen/SVD>\n\n#include <Eigen/SparseCore>\n#include <Eigen/QR>\n#include <Eigen/SPQRSupport>\n\n#include <unordered_set>\n\n#include \"State.h\"\n#include \"StateManager.h\"\n\n#include \"MapServer.h\"\n#include \"MapServerManager.h\"\n\n#include \"Triangulator.h\"\n\n#include \"KeyframeUpdate.h\"\n\nnamespace ingvio\n{\n    int KeyframeUpdate::_select_cnt = 0;\n    \n    void KeyframeUpdate::getMargKfs(const std::shared_ptr<State> state,\n                                    std::vector<double>& marg_kfs)\n    {\n        if (state->_sw_camleft_poses.size() < _max_sw_poses || _max_sw_poses < 3)\n        {\n            marg_kfs.clear();\n            return;\n        }\n        \n        if (state->_timestamp == _timestamp && _kfs.size() > 0)\n        {\n            assert(_kfs.size() == 2);\n            marg_kfs = _kfs;\n            \n            return;\n        }\n        \n        if (state->_sw_camleft_poses.size() > _max_sw_poses)\n        {\n            std::cout << \"[KeyframeUpdate]: Current sw poses larger than max size!\" << std::endl;\n            std::exit(EXIT_FAILURE);\n        }\n        \n        _timestamp = state->_timestamp;\n        _kfs.clear();\n        \n        const int rem = _max_sw_poses - 2;\n        \n        const int idx1 = 2 + KeyframeUpdate::_select_cnt;\n        ++KeyframeUpdate::_select_cnt;\n        \n        KeyframeUpdate::_select_cnt = KeyframeUpdate::_select_cnt % rem;\n        \n        auto item1 = state->_sw_camleft_poses.rbegin();\n        for (int i = 0; i < idx1; ++i)\n            ++item1;\n        \n        auto item2 = state->_sw_camleft_poses.rbegin();\n        ++item2;\n        \n        _kfs.push_back(item1->first);\n        _kfs.push_back(item2->first);\n        \n        /*\n        auto item2 = state->_sw_camleft_poses.rbegin();\n        for (int i = 0; i < 2; ++i)\n            ++item2;\n        \n        double time1;\n        \n        if (KeyframeUpdate::_select_cnt == 0)\n        {\n            auto item1 = state->_sw_camleft_poses.begin();\n            for (int i = 0; i < 2; ++i)\n                ++item1;\n            time1 = item1->first;\n        }\n        else\n        {\n            auto item1 = state->_sw_camleft_poses.rbegin();\n            for (int i = 0; i < 3; ++i)\n                ++item1;\n            time1 = item1->first;\n        }\n        \n        _kfs.push_back(time1);\n        _kfs.push_back(item2->first);\n        \n        ++KeyframeUpdate::_select_cnt;\n        KeyframeUpdate::_select_cnt = KeyframeUpdate::_select_cnt % 2;\n        */\n        \n        marg_kfs = _kfs;\n    }\n    \n    void KeyframeUpdate::margSwPose(std::shared_ptr<State> state)\n    {\n        std::vector<double> marg_kfs;\n        \n        this->getMargKfs(state, marg_kfs);\n        \n        if (marg_kfs.size() == 0)\n            return;\n        \n        for (const double& marg_time : marg_kfs)\n            StateManager::margSlidingWindowPose(state, marg_time);\n    }\n    \n    void KeyframeUpdate::generateSwVarOrder(const std::shared_ptr<State> state,\n                                            const std::vector<double>& selected_timestamps,\n                                            std::vector<std::shared_ptr<SE3>>& sw_var_order,\n                                            std::map<std::shared_ptr<SE3>, int>& sw_index,\n                                            std::vector<std::shared_ptr<Type>>& sw_var_type)\n    {\n        sw_var_order.clear();\n        sw_index.clear();\n        sw_var_type.clear();\n        \n        int cnt = 0;\n        \n        const auto& sw_poses = state->_sw_camleft_poses;\n        \n        for (const double& ts : selected_timestamps)\n        {\n            if (sw_poses.find(ts) == sw_poses.end())\n            {\n                std::cout << \"[KeyframeUpdate]: selected timestamp not in sw!\" << std::endl;\n                std::exit(EXIT_FAILURE);\n            }\n            \n            sw_var_order.push_back(sw_poses.at(ts));\n            sw_var_type.push_back(sw_poses.at(ts));\n            sw_index[sw_poses.at(ts)] = 6*cnt;\n            ++cnt;\n        }\n    }\n    \n    std::shared_ptr<SE3> KeyframeUpdate::calcResJacobianSingleFeatSelectedMonoObs(\n        const std::shared_ptr<FeatureInfo> feature_info,\n        const std::map<double, std::shared_ptr<SE3>>& sw_poses,\n        const std::vector<std::shared_ptr<SE3>>& sw_var_order,\n        const std::map<std::shared_ptr<SE3>, int>& sw_index_map,\n        const std::vector<double>& selected_timestamps,\n        Eigen::VectorXd& res_block,\n        Eigen::MatrixXd& H_block,\n        Eigen::MatrixXd& H_anchor_block)\n    {\n        const int num_of_cols = 6*sw_var_order.size();\n        const int num_of_rows = 2*selected_timestamps.size();\n        \n        Eigen::VectorXd res_block_tmp = Eigen::VectorXd::Zero(num_of_rows);\n        Eigen::MatrixXd H_block_tmp = Eigen::MatrixXd::Zero(num_of_rows, num_of_cols);\n        Eigen::MatrixXd H_anchor_block_tmp = Eigen::MatrixXd::Zero(num_of_rows, 6);\n        Eigen::MatrixXd Hf_block_tmp = Eigen::MatrixXd::Zero(num_of_rows, 3);\n        \n        const Eigen::Vector3d pf_w = feature_info->_landmark->valuePosXyz();\n        const std::shared_ptr<SE3> pose_anchor_ptr = feature_info->_landmark->getAnchoredPose();\n        \n        int row_cnt = 0;\n        \n        for (const auto& time_of_obs : selected_timestamps)\n        {\n            if (sw_poses.find(time_of_obs) == sw_poses.end() || feature_info->_mono_obs.find(time_of_obs) == feature_info->_mono_obs.end())\n                continue;\n            \n            const std::shared_ptr<MonoMeas>& mono_obs_ptr = feature_info->_mono_obs.at(time_of_obs);\n            const std::shared_ptr<SE3>& pose_obs_ptr = sw_poses.at(time_of_obs);\n            \n            \n            const Eigen::Matrix3d R_cm2w = pose_obs_ptr->valueLinearAsMat();\n            const Eigen::Vector3d p_cm = pose_obs_ptr->valueTrans();\n            \n            const Eigen::Vector3d pf_cm = R_cm2w.transpose()*(pf_w-p_cm);\n            \n            Eigen::Matrix<double, 2, 3> H_proj = Eigen::Matrix<double, 2, 3>::Zero();\n            H_proj(0, 0) = 1.0/pf_cm.z();\n            H_proj(0, 2) = -pf_cm.x()/std::pow(pf_cm.z(), 2);\n            H_proj(1, 1) = 1.0/pf_cm.z();\n            H_proj(1, 2) = -pf_cm.y()/std::pow(pf_cm.z(), 2);\n            \n            Eigen::MatrixXd H_pf2x = Eigen::MatrixXd::Zero(3, num_of_cols);\n            Eigen::Matrix<double, 3, 6> H_pf2anchor = Eigen::Matrix<double, 3, 6>::Zero();\n            \n            if (pose_obs_ptr != pose_anchor_ptr)\n            {\n                H_pf2x.block<3, 3>(0, sw_index_map.at(pose_obs_ptr)) = R_cm2w.transpose()*skew(pf_w);\n        \n                H_pf2anchor.block<3, 3>(0, 0) = -H_pf2x.block<3, 3>(0, sw_index_map.at(pose_obs_ptr));\n            }\n            \n            H_pf2x.block<3, 3>(0, sw_index_map.at(pose_obs_ptr)+3) = -R_cm2w.transpose();\n            Eigen::Matrix3d H_pf2pf = R_cm2w.transpose();\n            \n            if (H_proj.hasNaN() || H_pf2x.hasNaN()) \n                continue;\n            \n            H_block_tmp.block(row_cnt, 0, 2, num_of_cols) = H_proj*H_pf2x;\n            \n            H_anchor_block_tmp.block(row_cnt, 0, 2, 6) = H_proj*H_pf2anchor;\n            \n            Hf_block_tmp.block(row_cnt, 0, 2, 3) = H_proj*H_pf2pf;\n            \n            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());\n            \n            row_cnt += 2;\n        }\n        \n        if (row_cnt < res_block.rows())\n        {\n            res_block_tmp.conservativeResize(row_cnt, 1);\n            H_block_tmp.conservativeResize(row_cnt, num_of_cols);\n            H_anchor_block_tmp.conservativeResize(row_cnt, 6);\n            Hf_block_tmp.conservativeResize(row_cnt, 3);\n        }\n        \n        if (row_cnt <= 3)\n            std::cout << \"[KeyframeUpdate]: Warning! in calc one feat selected jacobian, num of rows <= 3 !\" << std::endl;\n        \n        Eigen::JacobiSVD<Eigen::MatrixXd> svd_helper(Hf_block_tmp, Eigen::ComputeFullU | Eigen::ComputeThinV);\n        Eigen::MatrixXd V = svd_helper.matrixU().rightCols(row_cnt-3);\n        \n        H_block = V.transpose()*H_block_tmp;\n        res_block = V.transpose()*res_block_tmp;\n        H_anchor_block = V.transpose()*H_anchor_block_tmp;\n        \n        return pose_anchor_ptr;\n    }\n    \n    void KeyframeUpdate::cleanMonoObsAtMargTime(std::shared_ptr<State> state,\n                                                std::shared_ptr<MapServer> map_server)\n    {\n        std::vector<double> marg_kfs;\n        \n        this->getMargKfs(state, marg_kfs);\n        \n        if (marg_kfs.size() == 0)\n            return;\n        \n        std::vector<int> all_ids;\n        for (const auto& item : *map_server)\n            all_ids.push_back(item.first);\n        \n        std::vector<int> ids_to_clean;\n        for (const int& id : all_ids)\n            for (const double& marg_time : marg_kfs)\n            {\n                if (map_server->at(id)->_mono_obs.find(marg_time) != map_server->at(id)->_mono_obs.end())\n                    map_server->at(id)->_mono_obs.erase(marg_time);\n            \n                if (map_server->at(id)->_mono_obs.size() == 0)\n                    ids_to_clean.push_back(id);\n            }\n        \n        for (int id : ids_to_clean)\n            map_server->erase(id);\n    }\n    \n    void KeyframeUpdate::changeMSCKFAnchor(std::shared_ptr<State> state,\n                                           std::shared_ptr<MapServer> map_server)\n    {\n        std::vector<double> marg_kfs;\n        \n        this->getMargKfs(state, marg_kfs);\n        \n        if (marg_kfs.size() == 0)\n            return;\n        \n        std::unordered_set<std::shared_ptr<SE3>> old_anchor_set;\n        \n        for (const double& marg_time : marg_kfs)\n            old_anchor_set.insert(state->_sw_camleft_poses.at(marg_time));\n               \n        const std::shared_ptr<SE3> new_anchor = state->_sw_camleft_poses.rbegin()->second;\n        \n        std::vector<int> ids_to_marg;\n        \n        for (auto& item : *map_server)\n        {\n            if (item.second->_ftype != FeatureInfo::FeatureType::MSCKF)\n                continue;\n            \n            if (old_anchor_set.find(item.second->_landmark->getAnchoredPose()) !=\n                old_anchor_set.end())\n            {\n                \n                if (item.second->_isTri)\n                {\n                    const Eigen::Vector3d pf = item.second->_landmark->valuePosXyz();\n                    const Eigen::Vector3d body = new_anchor->valueLinearAsMat().transpose()*(pf - new_anchor->valueTrans());\n                    \n                    if (body.z() <= 0.3)\n                    {\n                        ids_to_marg.push_back(item.first);\n                        continue;\n                    }\n                    \n                    item.second->_landmark->resetAnchoredPose(new_anchor, true);\n                }\n                else\n                    ids_to_marg.push_back(item.first);\n            }\n        }\n        \n        for (const int& id: ids_to_marg)\n            map_server->erase(id);\n    }\n    \n    std::shared_ptr<SE3> KeyframeUpdate::calcResJacobianSingleFeatSelectedStereoObs(\n        const std::shared_ptr<FeatureInfo> feature_info, \n        const std::map<double, std::shared_ptr<SE3>>& sw_poses,\n        const std::vector<std::shared_ptr<SE3>>& sw_var_order,\n        const std::map<std::shared_ptr<SE3>, int>& sw_index_map,\n        const std::vector<double>& selected_timestamps,\n        const Eigen::Isometry3d& T_cl2cr,\n        Eigen::VectorXd& res_block,\n        Eigen::MatrixXd& H_block,\n        Eigen::MatrixXd& H_anchor_block)\n    {\n        \n        const Eigen::Matrix3d& R_cl2cr = T_cl2cr.linear();\n        \n        const int num_of_cols = 6*sw_var_order.size();\n        const int num_of_rows = 4*selected_timestamps.size();\n        \n        Eigen::VectorXd res_block_tmp = Eigen::VectorXd::Zero(num_of_rows);\n        Eigen::MatrixXd H_block_tmp = Eigen::MatrixXd::Zero(num_of_rows, num_of_cols);\n        Eigen::MatrixXd H_anchor_block_tmp = Eigen::MatrixXd::Zero(num_of_rows, 6);\n        Eigen::MatrixXd Hf_block_tmp = Eigen::MatrixXd::Zero(num_of_rows, 3);\n        \n        const Eigen::Vector3d pf_w = feature_info->_landmark->valuePosXyz();\n        const std::shared_ptr<SE3> pose_anchor_ptr = feature_info->_landmark->getAnchoredPose();\n        \n        int row_cnt = 0;\n        \n        for (const auto& time_of_obs : selected_timestamps)\n        {\n            if (sw_poses.find(time_of_obs) == sw_poses.end() || feature_info->_stereo_obs.find(time_of_obs) == feature_info->_stereo_obs.end())\n                continue;\n            \n            const std::shared_ptr<StereoMeas>& stereo_obs_ptr = feature_info->_stereo_obs.at(time_of_obs);\n            const std::shared_ptr<SE3>& pose_obs_ptr = sw_poses.at(time_of_obs);\n            \n            const Eigen::Matrix3d R_cm2w = pose_obs_ptr->valueLinearAsMat();\n            const Eigen::Vector3d p_cm = pose_obs_ptr->valueTrans();\n            \n            const Eigen::Vector3d pf_cm = R_cm2w.transpose()*(pf_w-p_cm);\n            \n            Eigen::Matrix<double, 2, 3> H_proj = Eigen::Matrix<double, 2, 3>::Zero();\n            H_proj(0, 0) = 1.0/pf_cm.z();\n            H_proj(0, 2) = -pf_cm.x()/std::pow(pf_cm.z(), 2);\n            H_proj(1, 1) = 1.0/pf_cm.z();\n            H_proj(1, 2) = -pf_cm.y()/std::pow(pf_cm.z(), 2);\n            \n            const Eigen::Vector3d pf_cm_r = T_cl2cr*pf_cm;\n            \n            Eigen::Matrix<double, 2, 3> H_proj_r = Eigen::Matrix<double, 2, 3>::Zero();\n            H_proj_r(0, 0) = 1.0/pf_cm_r.z();\n            H_proj_r(0, 2) = -pf_cm_r.x()/std::pow(pf_cm_r.z(), 2);\n            H_proj_r(1, 1) = 1.0/pf_cm_r.z();\n            H_proj_r(1, 2) = -pf_cm_r.y()/std::pow(pf_cm_r.z(), 2);\n            \n            Eigen::MatrixXd H_pf2x = Eigen::MatrixXd::Zero(3, num_of_cols);\n            Eigen::Matrix<double, 3, 6> H_pf2anchor = Eigen::Matrix<double, 3, 6>::Zero();\n            \n            if (pose_anchor_ptr != pose_obs_ptr)\n            {\n                H_pf2x.block<3, 3>(0, sw_index_map.at(pose_obs_ptr)) = R_cm2w.transpose()*skew(pf_w);\n            \n                H_pf2anchor.block<3, 3>(0, 0) = -H_pf2x.block<3, 3>(0, sw_index_map.at(pose_obs_ptr));\n            }\n            \n            H_pf2x.block<3, 3>(0, sw_index_map.at(pose_obs_ptr)+3) = -R_cm2w.transpose();\n            Eigen::Matrix3d H_pf2pf = R_cm2w.transpose();\n            \n            if (H_proj.hasNaN() || H_pf2x.hasNaN()) \n                continue;\n            \n            H_block_tmp.block(row_cnt, 0, 2, num_of_cols) = H_proj*H_pf2x;\n            \n            H_anchor_block_tmp.block(row_cnt, 0, 2, 6) = H_proj*H_pf2anchor;\n            \n            Hf_block_tmp.block(row_cnt, 0, 2, 3) = H_proj*H_pf2pf;\n            \n            H_block_tmp.block(row_cnt+2, 0, 2, num_of_cols) = H_proj_r*R_cl2cr*H_pf2x;\n            \n            H_anchor_block_tmp.block(row_cnt+2, 0, 2, 6) = H_proj_r*R_cl2cr*H_pf2anchor;\n            \n            Hf_block_tmp.block(row_cnt+2, 0, 2, 3) = H_proj_r*R_cl2cr*H_pf2pf;\n            \n            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());\n            \n            row_cnt += 4;\n        }\n        \n        if (row_cnt < res_block.rows())\n        {\n            res_block_tmp.conservativeResize(row_cnt, 1);\n            H_block_tmp.conservativeResize(row_cnt, num_of_cols);\n            H_anchor_block_tmp.conservativeResize(row_cnt, 6);\n            Hf_block_tmp.conservativeResize(row_cnt, 3);\n        }\n        \n        if (row_cnt <= 3)\n            std::cout << \"[KeyframeUpdate]: Warning! in calc one feat selected jacobian, num of rows <= 3 !\" << std::endl;\n        \n        Eigen::JacobiSVD<Eigen::MatrixXd> svd_helper(Hf_block_tmp, Eigen::ComputeFullU | Eigen::ComputeThinV);\n        Eigen::MatrixXd V = svd_helper.matrixU().rightCols(row_cnt-3);\n        \n        H_block = V.transpose()*H_block_tmp;\n        H_anchor_block = V.transpose()*H_anchor_block_tmp;\n        res_block = V.transpose()*res_block_tmp;\n        \n        return pose_anchor_ptr;\n    }\n    \n    void KeyframeUpdate::updateStateMono(std::shared_ptr<State> state,\n                                         std::shared_ptr<MapServer> map_server,\n                                         std::shared_ptr<Triangulator> tri)\n    {\n        std::vector<double> selected_timestamps;\n        \n        this->getMargKfs(state, selected_timestamps);\n        \n        if (selected_timestamps.size() == 0)\n            return;\n        \n        std::vector<std::shared_ptr<SE3>> sw_var_order;\n        std::map<std::shared_ptr<SE3>, int> sw_index_map;\n        std::vector<std::shared_ptr<Type>> sw_var_type;\n        \n        this->generateSwVarOrder(state, selected_timestamps, sw_var_order,\n                                 sw_index_map, sw_var_type);\n        \n        std::vector<int> update_ids;\n        \n        for (const auto& item : *map_server)\n        {\n            const auto& feat_info_ptr = item.second;\n            \n            if (feat_info_ptr->_ftype != FeatureInfo::FeatureType::MSCKF)\n                continue;\n            \n            bool flag = false;\n            const auto& mono_obs = item.second->_mono_obs;\n            \n            for (const double& ts : selected_timestamps)\n                if (mono_obs.find(ts) == mono_obs.end())\n                {\n                    flag = true;\n                    break;\n                }\n                \n            if (flag) continue;\n                \n            if (FeatureInfoManager::triangulateFeatureInfoMono(feat_info_ptr, tri, state))\n                update_ids.push_back(item.first);\n        }\n        \n        if (update_ids.size() == 0) return;\n        \n        const int max_possible_cols = 6*state->_sw_camleft_poses.size();\n        \n        const int max_possible_rows = update_ids.size()*(2*selected_timestamps.size()-3);\n        \n        Eigen::VectorXd res_large(max_possible_rows);\n        Eigen::MatrixXd H_large(max_possible_rows, max_possible_cols);\n        res_large.setZero();\n        H_large.setZero();\n        \n        int row_cnt = 0;\n        int col_cnt = 6*sw_var_type.size();\n        \n        int feats_cnt = 0;\n        \n        for (int i = 0; i < update_ids.size(); ++i)\n        {\n            Eigen::VectorXd res_block;\n            Eigen::MatrixXd H_block;\n            Eigen::MatrixXd H_anchor_block;\n            \n            const auto& anchor_pose_ptr = this->calcResJacobianSingleFeatSelectedMonoObs(\n                map_server->at(update_ids[i]), state->_sw_camleft_poses,\n                sw_var_order, sw_index_map, selected_timestamps,\n                res_block, H_block, H_anchor_block);\n            \n            bool flag = false;\n            \n            if (sw_index_map.find(anchor_pose_ptr) == sw_index_map.end())\n            {\n                flag = true;\n                \n                sw_var_order.push_back(anchor_pose_ptr);\n                sw_var_type.push_back(anchor_pose_ptr);\n                sw_index_map[anchor_pose_ptr] = col_cnt;\n                \n                col_cnt += anchor_pose_ptr->size();\n                \n                H_block.conservativeResize(H_block.rows(), H_block.cols() + 6);\n            }\n            \n            H_block.block(0, sw_index_map.at(anchor_pose_ptr), H_block.rows(), 6) = H_anchor_block;\n            \n            if (!testChiSquared(state, res_block, H_block, \n                                sw_var_type, this->_noise, 2))\n            {\n                if (flag)\n                {\n                    sw_var_order.pop_back();\n                    sw_var_type.pop_back();\n                    sw_index_map.erase(anchor_pose_ptr);\n                    \n                    col_cnt -= anchor_pose_ptr->size();\n                }\n                \n                continue;\n            }\n            \n            H_large.block(row_cnt, 0, H_block.rows(), col_cnt) = H_block;\n            res_large.block(row_cnt, 0, res_block.rows(), 1) = res_block;\n            \n            row_cnt += res_block.rows();\n            \n            ++feats_cnt;\n        }\n        \n        if (row_cnt < max_possible_rows || col_cnt < max_possible_cols)\n        {\n            H_large.conservativeResize(row_cnt, col_cnt);\n            res_large.conservativeResize(row_cnt, 1);\n        }\n        \n        Eigen::MatrixXd H_thin;\n        Eigen::VectorXd res_thin;\n        \n        if (H_large.rows() > H_large.cols())\n        {\n            Eigen::SparseMatrix<double> H_sparse = H_large.sparseView();\n            \n            Eigen::SPQR<Eigen::SparseMatrix<double>> spqr_helper;\n            spqr_helper.setSPQROrdering(SPQR_ORDERING_NATURAL);\n            spqr_helper.compute(H_sparse);\n            \n            Eigen::MatrixXd H_temp;\n            Eigen::VectorXd r_temp;\n            \n            (spqr_helper.matrixQ().transpose()*H_large).evalTo(H_temp);\n            (spqr_helper.matrixQ().transpose()*res_large).evalTo(r_temp);\n            \n            H_thin = H_temp.topRows(col_cnt);\n            res_thin = r_temp.head(col_cnt);                 \n        }\n        else\n        {\n            H_thin = H_large;\n            res_thin = res_large;\n        }\n        \n        /*\n         std::cout << \"[KeyframeUpdate]: Num of mono feats used in selected pose = \" << feats_cnt << std::endl;                                                 *\n         */\n        \n        StateManager::ekfUpdate(state, sw_var_type, H_thin, res_thin, std::pow(this->_noise, 2)*Eigen::MatrixXd::Identity(res_thin.rows(), res_thin.rows()));\n    }\n    \n    void KeyframeUpdate::updateStateStereo(std::shared_ptr<State> state,\n                                           std::shared_ptr<MapServer> map_server,\n                                           std::shared_ptr<Triangulator> tri)\n    {\n        std::vector<double> selected_timestamps;\n        \n        this->getMargKfs(state, selected_timestamps);\n        \n        if (selected_timestamps.size() == 0)\n            return;\n        \n        std::vector<std::shared_ptr<SE3>> sw_var_order;\n        std::map<std::shared_ptr<SE3>, int> sw_index_map;\n        std::vector<std::shared_ptr<Type>> sw_var_type;\n        \n        this->generateSwVarOrder(state, selected_timestamps, sw_var_order,\n                                 sw_index_map, sw_var_type);\n        \n        std::vector<int> update_ids;\n        \n        for (const auto& item : *map_server)\n        {\n            const auto& feat_info_ptr = item.second;\n            \n            if (feat_info_ptr->_ftype != FeatureInfo::FeatureType::MSCKF)\n                continue;\n            \n            bool flag = false;\n            const auto& stereo_obs = item.second->_stereo_obs;\n            \n            for (const double& ts : selected_timestamps)\n                if (stereo_obs.find(ts) == stereo_obs.end())\n                {\n                    flag = true;\n                    break;\n                }\n                \n            if (flag) continue;\n                \n            if (FeatureInfoManager::triangulateFeatureInfoStereo(feat_info_ptr, tri, state))\n                update_ids.push_back(item.first);\n        }\n        \n        if (update_ids.size() == 0) return;\n        \n        const int max_possible_cols = 6*state->_sw_camleft_poses.size();\n        \n        const int max_possible_rows = update_ids.size()*(4*selected_timestamps.size()-3);\n        \n        Eigen::VectorXd res_large(max_possible_rows);\n        Eigen::MatrixXd H_large(max_possible_rows, max_possible_cols);\n        res_large.setZero();\n        H_large.setZero();\n        \n        int row_cnt = 0;\n        int col_cnt = 6*sw_var_type.size();\n        \n        int feats_cnt = 0;\n        \n        for (int i = 0; i < update_ids.size(); ++i)\n        {\n            Eigen::VectorXd res_block;\n            Eigen::MatrixXd H_block;\n            Eigen::MatrixXd H_anchor_block;\n            \n            const auto& anchor_pose_ptr = this->calcResJacobianSingleFeatSelectedStereoObs(\n                map_server->at(update_ids[i]), state->_sw_camleft_poses,\n                sw_var_order, sw_index_map, selected_timestamps,\n                state->_state_params._T_cl2cr,\n                res_block, H_block, H_anchor_block);\n            \n            bool flag = false;\n            \n            if (sw_index_map.find(anchor_pose_ptr) == sw_index_map.end())\n            {\n                flag = true;\n                \n                sw_var_order.push_back(anchor_pose_ptr);\n                sw_var_type.push_back(anchor_pose_ptr);\n                sw_index_map[anchor_pose_ptr] = col_cnt;\n                \n                col_cnt += anchor_pose_ptr->size();\n                \n                H_block.conservativeResize(H_block.rows(), H_block.cols() + 6);\n            }\n            \n            H_block.block(0, sw_index_map.at(anchor_pose_ptr), H_block.rows(), 6) = H_anchor_block;\n            \n            if (!testChiSquared(state, res_block, H_block, sw_var_type, \n                this->_noise, 2))\n            {\n                if (flag)\n                {\n                    sw_var_order.pop_back();\n                    sw_var_type.pop_back();\n                    sw_index_map.erase(anchor_pose_ptr);\n                    \n                    col_cnt -= anchor_pose_ptr->size();\n                }\n                \n                continue;\n            }\n            \n            H_large.block(row_cnt, 0, H_block.rows(), col_cnt) = H_block;\n            res_large.block(row_cnt, 0, res_block.rows(), 1) = res_block;\n            \n            row_cnt += res_block.rows();\n            \n            ++feats_cnt;\n        }\n        \n        if (row_cnt < max_possible_rows || col_cnt < max_possible_cols)\n        {\n            H_large.conservativeResize(row_cnt, col_cnt);\n            res_large.conservativeResize(row_cnt, 1);\n        }\n        \n        Eigen::MatrixXd H_thin;\n        Eigen::VectorXd res_thin;\n        \n        if (H_large.rows() > H_large.cols())\n        {\n            Eigen::SparseMatrix<double> H_sparse = H_large.sparseView();\n            \n            Eigen::SPQR<Eigen::SparseMatrix<double>> spqr_helper;\n            spqr_helper.setSPQROrdering(SPQR_ORDERING_NATURAL);\n            spqr_helper.compute(H_sparse);\n            \n            Eigen::MatrixXd H_temp;\n            Eigen::VectorXd r_temp;\n            \n            (spqr_helper.matrixQ().transpose()*H_large).evalTo(H_temp);\n            (spqr_helper.matrixQ().transpose()*res_large).evalTo(r_temp);\n            \n            H_thin = H_temp.topRows(col_cnt);\n            res_thin = r_temp.head(col_cnt);                 \n        }\n        else\n        {\n            H_thin = H_large;\n            res_thin = res_large;\n        }\n        \n        /*\n         std::cout << \"[KeyframeUpdate]: Num of stereo feats used in selected pose = \" << feats_cnt << std::endl;                                              \n         */\n        \n        StateManager::ekfUpdate(state, sw_var_type, H_thin, res_thin, std::pow(this->_noise, 2)*Eigen::MatrixXd::Identity(res_thin.rows(), res_thin.rows()));\n    }\n    \n    void KeyframeUpdate::cleanStereoObsAtMargTime(std::shared_ptr<State> state,\n                                                  std::shared_ptr<MapServer> map_server)\n    {\n        std::vector<double> marg_kfs;\n        \n        this->getMargKfs(state, marg_kfs);\n        \n        std::vector<int> all_ids;\n        for (const auto& item : *map_server)\n            all_ids.push_back(item.first);\n        \n        std::vector<int> ids_to_clean;\n        for (const int& id : all_ids)\n            for (const double& marg_time : marg_kfs)\n            {\n                if (map_server->at(id)->_stereo_obs.find(marg_time) != map_server->at(id)->_stereo_obs.end())\n                    map_server->at(id)->_stereo_obs.erase(marg_time);\n                \n                if (map_server->at(id)->_stereo_obs.size() == 0)\n                    ids_to_clean.push_back(id);\n            }\n            \n        for (int id : ids_to_clean)\n                map_server->erase(id);\n    }\n}\n"
  },
  {
    "path": "ingvio_estimator/src/KeyframeUpdate.h",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#pragma once\n\n#include \"IngvioParams.h\"\n#include \"MapServer.h\"\n\n#include \"Update.h\"\n\nnamespace ingvio\n{\n    class State;\n    class Triangulator;\n    \n    class KeyframeUpdate : public UpdateBase\n    {\n    public:\n        using UpdateBase::UpdateBase;\n        \n        KeyframeUpdate(const IngvioParams& filter_params) :\n        UpdateBase(filter_params._chi2_max_dof, filter_params._chi2_thres),\n        _noise(filter_params._visual_noise),\n        _max_sw_poses(filter_params._max_sw_clones)\n        {}\n        \n        KeyframeUpdate(const KeyframeUpdate&) = delete;\n        KeyframeUpdate operator=(const KeyframeUpdate&) = delete;\n        \n        virtual ~KeyframeUpdate() {}\n        \n        void getMargKfs(const std::shared_ptr<State> state,\n                        std::vector<double>& marg_kfs);\n        \n        void margSwPose(std::shared_ptr<State> state);\n        \n        void changeMSCKFAnchor(std::shared_ptr<State> state,\n                               std::shared_ptr<MapServer> map_server);\n        \n        void updateStateMono(std::shared_ptr<State> state,\n                             std::shared_ptr<MapServer> map_server,\n                             std::shared_ptr<Triangulator> tri);\n        \n        void cleanMonoObsAtMargTime(std::shared_ptr<State> state,\n                                    std::shared_ptr<MapServer> map_server);\n        \n        void updateStateStereo(std::shared_ptr<State> state,\n                               std::shared_ptr<MapServer> map_server,\n                               std::shared_ptr<Triangulator> tri);\n        \n        void cleanStereoObsAtMargTime(std::shared_ptr<State> state,\n                                      std::shared_ptr<MapServer> map_server);\n        \n    protected:\n        double _noise;\n        int _max_sw_poses;\n        \n        double _timestamp = -1.0;\n        std::vector<double> _kfs;\n        \n        static int _select_cnt;\n        \n        void generateSwVarOrder(const std::shared_ptr<State> state,\n                                const std::vector<double>& selected_timestamps,\n                                std::vector<std::shared_ptr<SE3>>& sw_var_order,\n                                std::map<std::shared_ptr<SE3>, int>& sw_index,\n                                std::vector<std::shared_ptr<Type>>& sw_var_type);\n        \n        std::shared_ptr<SE3> calcResJacobianSingleFeatSelectedMonoObs(\n            const std::shared_ptr<FeatureInfo> feature_info,\n            const std::map<double, std::shared_ptr<SE3>>& sw_poses,\n            const std::vector<std::shared_ptr<SE3>>& sw_var_order,\n            const std::map<std::shared_ptr<SE3>, int>& sw_index_map,\n            const std::vector<double>& selected_timestamps,\n            Eigen::VectorXd& res_block,\n            Eigen::MatrixXd& H_block,\n            Eigen::MatrixXd& H_anchor_block);\n        \n        std::shared_ptr<SE3> calcResJacobianSingleFeatSelectedStereoObs(\n            const std::shared_ptr<FeatureInfo> feature_info, \n            const std::map<double, std::shared_ptr<SE3>>& sw_poses,\n            const std::vector<std::shared_ptr<SE3>>& sw_var_order,\n            const std::map<std::shared_ptr<SE3>, int>& sw_index_map,\n            const std::vector<double>& selected_timestamps,\n            const Eigen::Isometry3d& T_cl2cr,\n            Eigen::VectorXd& res_block,\n            Eigen::MatrixXd& H_block,\n            Eigen::MatrixXd& H_anchor_block);\n        \n    };\n}\n"
  },
  {
    "path": "ingvio_estimator/src/LandmarkUpdate.cpp",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#include <unordered_set>\n\n#include \"StateManager.h\"\n\n#include \"MapServer.h\"\n#include \"MapServerManager.h\"\n\n#include \"LandmarkUpdate.h\"\n\nnamespace ingvio\n{\n    void LandmarkUpdate::updateLandmarkMono(std::shared_ptr<State> state,\n                                            std::shared_ptr<MapServer> map_server)\n    {\n        if (state->_anchored_landmarks.size() == 0) return;\n        \n        std::vector<std::shared_ptr<Type>> var_order;\n        var_order.push_back(state->_extended_pose);\n        var_order.push_back(state->_camleft_imu_extrinsics);\n        \n        std::map<std::shared_ptr<Type>, int> sub_var_col_idx;\n        sub_var_col_idx[state->_extended_pose] = 0;\n        sub_var_col_idx[state->_camleft_imu_extrinsics] = 9;\n        \n        const int max_possible_rows = 2*state->_anchored_landmarks.size();\n        const int max_possible_cols = 15 + 6*state->_sw_camleft_poses.size() + 3*state->_anchored_landmarks.size();\n        \n        Eigen::MatrixXd H_large(max_possible_rows, max_possible_cols);\n        Eigen::VectorXd res_large(max_possible_rows);\n        H_large.setZero();\n        res_large.setZero();\n        \n        int row_cnt = 0;\n        int col_cnt = 15;\n        \n        for (const auto& item : state->_anchored_landmarks)\n        {\n            const int& id = item.first;\n            if (map_server->find(id) == map_server->end())\n            {\n                std::cout << \"[LandmarkUpdate]: Landmark in state not in map server!\" << std::endl;\n                std::exit(EXIT_FAILURE);\n            }\n            else if (map_server->at(id)->_ftype != FeatureInfo::FeatureType::SLAM)\n            {\n                std::cout << \"[LandmarkUpdate]: Landmark in state not marked SLAM type in map server!\" << std::endl;\n                std::exit(EXIT_FAILURE);\n            }\n            else if (map_server->at(id)->_mono_obs.find(state->_timestamp) == map_server->at(id)->_mono_obs.end())\n            {\n                std::cout << \"[LandmarkUpdate]: Landmark in state not tracked to curr time! Should have been marged before!\" << std::endl;\n                std::exit(EXIT_FAILURE);\n            }\n            \n            if (map_server->at(id)->_landmark != state->_anchored_landmarks.at(id))\n            {\n                std::cout << \"[LandmarkUpdate]: Landmark ptr in state not the same as that in map server!\" << std::endl;\n                std::exit(EXIT_FAILURE);\n            }\n            \n            std::shared_ptr<SE3> anchored_pose_ptr = item.second->getAnchoredPose();\n            std::shared_ptr<AnchoredLandmark> lm_ptr = item.second;\n            \n            Eigen::Vector2d res;\n            Eigen::Matrix<double, 2, 9> H_fj_epose;\n            Eigen::Matrix<double, 2, 6> H_fj_ext;\n            Eigen::Matrix<double, 2, 6> H_fj_anch;\n            Eigen::Matrix<double, 2, 3> H_fj_pf;\n            \n            this->calcResJacobianSingleLandmarkMono(map_server->at(id), state, res, H_fj_epose,\n                                                    H_fj_ext, H_fj_anch, H_fj_pf);\n            \n            std::vector<std::shared_ptr<Type>> var_order_chi2(4);\n            var_order_chi2[0] = state->_extended_pose;\n            var_order_chi2[1] = state->_camleft_imu_extrinsics;\n            var_order_chi2[2] = anchored_pose_ptr;\n            var_order_chi2[3] = lm_ptr;\n            \n            Eigen::MatrixXd H_chi2(2, 24);\n            H_chi2.block<2, 9>(0, 0) = H_fj_epose;\n            H_chi2.block<2, 6>(0, 9) = H_fj_ext;\n            H_chi2.block<2, 6>(0, 15) = H_fj_anch;\n            H_chi2.block<2, 3>(0, 21) = H_fj_pf;\n            \n            if (!testChiSquared(state, res, H_chi2, var_order_chi2, this->_noise))\n                continue;\n            \n            res_large.block<2, 1>(row_cnt, 0) = res;\n            \n            H_large.block<2, 9>(row_cnt, sub_var_col_idx.at(state->_extended_pose)) = H_fj_epose;\n            \n            H_large.block<2, 6>(row_cnt, sub_var_col_idx.at(state->_camleft_imu_extrinsics)) = H_fj_ext;\n            \n            if (sub_var_col_idx.find(anchored_pose_ptr) == sub_var_col_idx.end())\n            {\n                sub_var_col_idx[anchored_pose_ptr] = col_cnt;\n                col_cnt += 6;\n                var_order.push_back(anchored_pose_ptr);\n            }\n            \n            H_large.block<2, 6>(row_cnt, sub_var_col_idx.at(anchored_pose_ptr)) = H_fj_anch;\n            \n            if (sub_var_col_idx.find(lm_ptr) == sub_var_col_idx.end())\n            {\n                sub_var_col_idx[lm_ptr] = col_cnt;\n                col_cnt += 3;\n                var_order.push_back(lm_ptr);\n            }\n            \n            H_large.block<2, 3>(row_cnt, sub_var_col_idx.at(lm_ptr)) = H_fj_pf;\n            \n            row_cnt += 2;\n        }\n        \n        if (row_cnt == 0) return;\n        \n        /*\n        std::cout << \"[LandmarkUpdate]: row cnt = \" << row_cnt << std::endl;\n        std::cout << \"[LandmarkUpdate]: col cnt = \" << col_cnt << std::endl;\n        */\n        \n        if (max_possible_rows > row_cnt || max_possible_cols > col_cnt)\n        {\n            H_large.conservativeResize(row_cnt, col_cnt);\n            res_large.conservativeResize(row_cnt, 1);\n        }\n        \n        StateManager::ekfUpdate(state, var_order, H_large, res_large, std::pow(this->_noise, 2)*Eigen::MatrixXd::Identity(res_large.rows(), res_large.rows()));\n    }\n    \n    void LandmarkUpdate::updateLandmarkMonoSw(std::shared_ptr<State> state,\n                                            std::shared_ptr<MapServer> map_server)\n    {\n        if (state->_anchored_landmarks.size() == 0) return;\n        \n        if (state->_timestamp != state->_sw_camleft_poses.rbegin()->first)\n        {\n            std::cout << \"[LandmarkUpdate]: Last sw pose is not at curr time!\" << std::endl;\n            std::exit(EXIT_FAILURE);\n        }\n        \n        const std::shared_ptr<SE3> curr_pose = state->_sw_camleft_poses.rbegin()->second;\n        \n        std::vector<std::shared_ptr<Type>> var_order;\n        var_order.push_back(curr_pose);\n        \n        std::map<std::shared_ptr<Type>, int> sub_var_col_idx;\n        sub_var_col_idx[curr_pose] = 0;\n        \n        const int max_possible_rows = 2*state->_anchored_landmarks.size();\n        const int max_possible_cols = 6*state->_sw_camleft_poses.size() + 3*state->_anchored_landmarks.size();\n        \n        Eigen::MatrixXd H_large(max_possible_rows, max_possible_cols);\n        Eigen::VectorXd res_large(max_possible_rows);\n        H_large.setZero();\n        res_large.setZero();\n        \n        int row_cnt = 0;\n        int col_cnt = 6;\n        \n        for (const auto& item : state->_anchored_landmarks)\n        {\n            const int& id = item.first;\n            if (map_server->find(id) == map_server->end())\n            {\n                std::cout << \"[LandmarkUpdate]: Landmark in state not in map server!\" << std::endl;\n                std::exit(EXIT_FAILURE);\n            }\n            else if (map_server->at(id)->_ftype != FeatureInfo::FeatureType::SLAM)\n            {\n                std::cout << \"[LandmarkUpdate]: Landmark in state not marked SLAM type in map server!\" << std::endl;\n                std::exit(EXIT_FAILURE);\n            }\n            else if (map_server->at(id)->_mono_obs.find(state->_timestamp) == map_server->at(id)->_mono_obs.end())\n            {\n                std::cout << \"[LandmarkUpdate]: Landmark in state not tracked to curr time! Should have been marged before!\" << std::endl;\n                std::exit(EXIT_FAILURE);\n            }\n            \n            if (map_server->at(id)->_landmark != state->_anchored_landmarks.at(id))\n            {\n                std::cout << \"[LandmarkUpdate]: Landmark ptr in state not the same as that in map server!\" << std::endl;\n                std::exit(EXIT_FAILURE);\n            }\n            \n            std::shared_ptr<SE3> anchored_pose_ptr = item.second->getAnchoredPose();\n            std::shared_ptr<AnchoredLandmark> lm_ptr = item.second;\n            \n            Eigen::Vector2d res;\n            Eigen::Matrix<double, 2, 6> H_fj_pose;\n            Eigen::Matrix<double, 2, 6> H_fj_anch;\n            Eigen::Matrix<double, 2, 3> H_fj_pf;\n            \n            this->calcResJacobianSingleLandmarkMono(map_server->at(id), state, res, \n                                                    H_fj_pose, H_fj_anch, H_fj_pf);\n            \n            if (curr_pose == anchored_pose_ptr)\n            {\n                std::cout << \"[LandmarkUpdate]: Warning! Current pose is the same as anchored pose!\" << std::endl;\n                \n                continue;\n            }\n            \n            std::vector<std::shared_ptr<Type>> var_order_chi2(3);\n            var_order_chi2[0] = curr_pose;\n            var_order_chi2[1] = anchored_pose_ptr;\n            var_order_chi2[2] = lm_ptr;\n            \n            Eigen::MatrixXd H_chi2(2, 15);\n            H_chi2.block<2, 6>(0, 0) = H_fj_pose;\n            H_chi2.block<2, 6>(0, 6) = H_fj_anch;\n            H_chi2.block<2, 3>(0, 12) = H_fj_pf;\n            \n            if (!testChiSquared(state, res, H_chi2, var_order_chi2, this->_noise))\n                continue;\n            \n            res_large.block<2, 1>(row_cnt, 0) = res;\n            \n            H_large.block<2, 6>(row_cnt, sub_var_col_idx.at(curr_pose)) = H_fj_pose;\n            \n            if (sub_var_col_idx.find(anchored_pose_ptr) == sub_var_col_idx.end())\n            {\n                sub_var_col_idx[anchored_pose_ptr] = col_cnt;\n                col_cnt += 6;\n                var_order.push_back(anchored_pose_ptr);\n            }\n            \n            H_large.block<2, 6>(row_cnt, sub_var_col_idx.at(anchored_pose_ptr)) = H_fj_anch;\n            \n            if (sub_var_col_idx.find(lm_ptr) == sub_var_col_idx.end())\n            {\n                sub_var_col_idx[lm_ptr] = col_cnt;\n                col_cnt += 3;\n                var_order.push_back(lm_ptr);\n            }\n            \n            H_large.block<2, 3>(row_cnt, sub_var_col_idx.at(lm_ptr)) = H_fj_pf;\n            \n            row_cnt += 2;\n        }\n        \n        if (row_cnt == 0) return;\n        \n        if (max_possible_rows > row_cnt || max_possible_cols > col_cnt)\n        {\n            H_large.conservativeResize(row_cnt, col_cnt);\n            res_large.conservativeResize(row_cnt, 1);\n        }\n        \n        StateManager::ekfUpdate(state, var_order, H_large, res_large, std::pow(this->_noise, 2)*Eigen::MatrixXd::Identity(res_large.rows(), res_large.rows()));\n    }\n    \n    void LandmarkUpdate::changeLandmarkAnchor(std::shared_ptr<State> state,\n                                              std::shared_ptr<MapServer> map_server)\n    {\n        double marg_time = state->nextMargTime();\n        \n        if (marg_time == INFINITY || state->_sw_camleft_poses.find(marg_time) == state->_sw_camleft_poses.end()) return;\n        \n        const std::shared_ptr<SE3> old_anchor = state->_sw_camleft_poses.at(marg_time);\n        \n        const double latest_sw = state->_sw_camleft_poses.rbegin()->first;\n        \n        const std::shared_ptr<SE3> new_anchor = state->_sw_camleft_poses.at(latest_sw);\n        \n        std::vector<int> ids_to_marg;\n        \n        for (auto& item : *map_server)\n        {\n            if (item.second->_ftype != FeatureInfo::FeatureType::SLAM)\n                continue;\n            \n            if (item.second->_landmark->getAnchoredPose() == old_anchor)\n            {\n                const Eigen::Vector3d pf = item.second->_landmark->valuePosXyz();\n                const Eigen::Vector3d body = new_anchor->valueLinearAsMat().transpose()*(pf - new_anchor->valueTrans());\n                    \n                if (body.z() <= 0)\n                {\n                    ids_to_marg.push_back(item.first);\n                    continue;\n                }\n                    \n                FeatureInfoManager::changeAnchoredPose(item.second, state, latest_sw);\n                \n                if (item.second->_landmark->getAnchoredPose() != new_anchor)\n                    ids_to_marg.push_back(item.first);\n            }\n        }\n        \n        for (const int& id: ids_to_marg)\n        {\n            StateManager::margAnchoredLandmarkInState(state, id);\n            map_server->erase(id);\n        }\n    }\n    \n    void LandmarkUpdate::changeLandmarkAnchor(std::shared_ptr<State> state, \n                                              std::shared_ptr<MapServer> map_server,\n                                              const std::vector<double>& marg_kfs)\n    {\n        if (marg_kfs.size() == 0)\n            return;\n        \n        std::unordered_set<std::shared_ptr<SE3>> old_anchor_set;\n        for (const double& marg : marg_kfs)\n            old_anchor_set.insert(state->_sw_camleft_poses.at(marg));\n        \n        const std::shared_ptr<SE3> new_anchor = state->_sw_camleft_poses.rbegin()->second;\n        \n        std::vector<int> ids_to_marg;\n        \n        for (auto& item : *map_server)\n        {\n            if (item.second->_ftype != FeatureInfo::FeatureType::SLAM)\n                continue;\n            \n            if (old_anchor_set.find(item.second->_landmark->getAnchoredPose()) != old_anchor_set.end())\n            {\n                const Eigen::Vector3d pf = item.second->_landmark->valuePosXyz();\n                const Eigen::Vector3d body = new_anchor->valueLinearAsMat().transpose()*(pf - new_anchor->valueTrans());\n                \n                if (body.z() <= 0)\n                {\n                    ids_to_marg.push_back(item.first);\n                    continue;\n                }\n                \n                FeatureInfoManager::changeAnchoredPose(item.second, state, state->_sw_camleft_poses.rbegin()->first);\n                \n                if (item.second->_landmark->getAnchoredPose() != new_anchor)\n                    ids_to_marg.push_back(item.first);\n            }\n        }\n        \n        for (const int& id: ids_to_marg)\n        {\n            StateManager::margAnchoredLandmarkInState(state, id);\n            map_server->erase(id);\n        }\n    }\n    \n    void LandmarkUpdate::initNewLandmarkMono(std::shared_ptr<State> state,\n                                             std::shared_ptr<MapServer> map_server,\n                                             std::shared_ptr<Triangulator> tri,\n                                             int min_init_poses)\n    {\n        if (state->_sw_camleft_poses.size() < min_init_poses)\n            return;\n        \n        const int vac_num_lm = state->_state_params._max_landmarks - state->_anchored_landmarks.size();\n        \n        if (vac_num_lm <= 0) return;\n        \n        std::vector<int> ids_to_init;\n        \n        for (const auto& item : *map_server)\n        {\n            if (ids_to_init.size() >= vac_num_lm) break;\n            \n            if (item.second->_mono_obs.size() < min_init_poses || \n                item.second->_ftype == FeatureInfo::FeatureType::SLAM)\n                continue;\n            \n            if (!FeatureInfoManager::triangulateFeatureInfoMono(item.second, tri, state))\n                continue;\n            \n            ids_to_init.push_back(item.first);\n        }\n        \n        if (ids_to_init.size() == 0) return;\n        \n        std::vector<std::shared_ptr<SE3>> sw_var_order;\n        std::map<std::shared_ptr<SE3>, int> sw_index_map;\n        std::vector<std::shared_ptr<Type>> sw_var_type;\n        \n        this->generateSwVarOrder(state, sw_var_order, sw_index_map, sw_var_type);\n        \n        for (const int& id : ids_to_init)\n        {\n            Eigen::VectorXd res_block;\n            Eigen::MatrixXd Hx_block;\n            Eigen::MatrixXd Hf_block;\n            \n            this->calcResJacobianSingleFeatAllMonoObs(map_server->at(id), state->_sw_camleft_poses, sw_var_order, sw_index_map, res_block, Hx_block, Hf_block);\n            \n            if (!StateManager::addVariableDelayed(state, map_server->at(id)->_landmark,\n                                                  sw_var_type, Hx_block, Hf_block, res_block,\n                                                  this->_noise, 0.95, true))\n                continue;\n            \n            if (state->_anchored_landmarks.find(id) != state->_anchored_landmarks.end())\n            {\n                std::cout << \"[LandmarkUpdate]: The id intended to add already in state!\" << std::endl;\n                std::exit(EXIT_FAILURE);\n            }\n            \n            state->_anchored_landmarks[id] = map_server->at(id)->_landmark;\n            \n            map_server->at(id)->_ftype = FeatureInfo::FeatureType::SLAM;\n        }\n        \n    }\n    \n    \n    void LandmarkUpdate::calcResJacobianSingleFeatAllMonoObs(\n        const std::shared_ptr<FeatureInfo> feature_info,\n        const std::map<double, std::shared_ptr<SE3>>& sw_poses,\n        const std::vector<std::shared_ptr<SE3>>& sw_var_order,\n        const std::map<std::shared_ptr<SE3>, int>& sw_index_map,\n        Eigen::VectorXd& res_block,\n        Eigen::MatrixXd& Hx_block,\n        Eigen::MatrixXd& Hf_block)\n    {\n        const int num_of_cols = 6*sw_var_order.size();\n        const int num_of_rows = 2*feature_info->numOfMonoFrames();\n        \n        Eigen::VectorXd res_block_tmp = Eigen::VectorXd::Zero(num_of_rows);\n        Eigen::MatrixXd H_block_tmp = Eigen::MatrixXd::Zero(num_of_rows, num_of_cols);\n        Eigen::MatrixXd Hf_block_tmp = Eigen::MatrixXd::Zero(num_of_rows, 3);\n        \n        const Eigen::Vector3d pf_w = feature_info->_landmark->valuePosXyz();\n        \n        int row_cnt = 0;\n        \n        for (const auto& item : feature_info->_mono_obs)\n        {\n            const double& time_of_obs = item.first;\n            \n            if (sw_poses.find(time_of_obs) == sw_poses.end())\n                continue;\n            \n            const std::shared_ptr<MonoMeas>& mono_obs_ptr = item.second;\n            const std::shared_ptr<SE3>& pose_obs_ptr = sw_poses.at(time_of_obs);\n            const std::shared_ptr<SE3> pose_anchor_ptr = feature_info->_landmark->getAnchoredPose();\n            \n            const Eigen::Matrix3d R_cm2w = pose_obs_ptr->valueLinearAsMat();\n            const Eigen::Vector3d p_cm = pose_obs_ptr->valueTrans();\n            \n            const Eigen::Vector3d pf_cm = R_cm2w.transpose()*(pf_w-p_cm);\n            \n            Eigen::Matrix<double, 2, 3> H_proj = Eigen::Matrix<double, 2, 3>::Zero();\n            H_proj(0, 0) = 1.0/pf_cm.z();\n            H_proj(0, 2) = -pf_cm.x()/std::pow(pf_cm.z(), 2);\n            H_proj(1, 1) = 1.0/pf_cm.z();\n            H_proj(1, 2) = -pf_cm.y()/std::pow(pf_cm.z(), 2);\n            \n            Eigen::MatrixXd H_pf2x = Eigen::MatrixXd::Zero(3, num_of_cols);\n            \n            if (pose_obs_ptr != pose_anchor_ptr)\n            {\n                H_pf2x.block<3, 3>(0, sw_index_map.at(pose_obs_ptr)) = R_cm2w.transpose()*skew(pf_w);\n                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));\n            }\n            \n            H_pf2x.block<3, 3>(0, sw_index_map.at(pose_obs_ptr)+3) = -R_cm2w.transpose();\n            Eigen::Matrix3d H_pf2pf = R_cm2w.transpose();\n            \n            if (H_proj.hasNaN() || H_pf2x.hasNaN()) \n                continue;\n            \n            H_block_tmp.block(row_cnt, 0, 2, num_of_cols) = H_proj*H_pf2x;\n            Hf_block_tmp.block(row_cnt, 0, 2, 3) = H_proj*H_pf2pf;\n            \n            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());\n            \n            row_cnt += 2;\n        }\n        \n        if (row_cnt < res_block.rows())\n        {\n            res_block_tmp.conservativeResize(row_cnt, 1);\n            H_block_tmp.conservativeResize(row_cnt, num_of_cols);\n            Hf_block_tmp.conservativeResize(row_cnt, 3);\n        }\n        \n        res_block = res_block_tmp;\n        Hx_block = H_block_tmp;\n        Hf_block = Hf_block_tmp;\n    }\n    \n    void LandmarkUpdate::generateSwVarOrder(const std::shared_ptr<State> state,\n                                            std::vector<std::shared_ptr<SE3>>& sw_var_order,\n                                            std::map<std::shared_ptr<SE3>, int>& sw_index,\n                                            std::vector<std::shared_ptr<Type>>& sw_var_type)\n    {\n        sw_var_order.clear();\n        sw_index.clear();\n        sw_var_type.clear();\n        \n        int cnt = 0;\n        for (const auto& item : state->_sw_camleft_poses)\n        {\n            sw_var_order.push_back(item.second);\n            sw_var_type.push_back(item.second);\n            sw_index[item.second] = 6*cnt;\n            ++cnt;\n        }\n    }\n    \n    void LandmarkUpdate::calcResJacobianSingleLandmarkMono(\n        const std::shared_ptr<FeatureInfo> feature_info,\n        const std::shared_ptr<State> state,\n        Eigen::Vector2d& res,\n        Eigen::Matrix<double, 2, 9>& H_fj_epose,\n        Eigen::Matrix<double, 2, 6>& H_fj_ext,\n        Eigen::Matrix<double, 2, 6>& H_fj_anch,\n        Eigen::Matrix<double, 2, 3>& H_fj_pf)\n    {\n        const Eigen::Vector3d pf_w = feature_info->_landmark->valuePosXyz();\n        \n        const Eigen::Matrix3d R_i2w_T = state->_extended_pose->valueLinearAsMat().transpose();\n        const Eigen::Matrix3d R_cl2i_T = state->_camleft_imu_extrinsics->valueLinearAsMat().transpose();\n        \n        const Eigen::Vector3d p_i2w = state->_extended_pose->valueTrans1();\n        const Eigen::Vector3d p_c2i = state->_camleft_imu_extrinsics->valueTrans();\n        \n        const Eigen::Vector3d pf_i = R_i2w_T*(pf_w-p_i2w);\n        const Eigen::Vector3d pf_cl = R_cl2i_T*(pf_i-p_c2i);\n        \n        const double curr_time = state->_timestamp;\n        \n        if (feature_info->_mono_obs.find(curr_time) == feature_info->_mono_obs.end() ||\n            feature_info->_ftype != FeatureInfo::FeatureType::SLAM)\n        {\n            std::cout << \"[LandmarkUpdate]: Cannot calc curr slam feature mono res and jacobi!\" << std::endl;\n            std::exit(EXIT_FAILURE);\n        }\n        \n        res = feature_info->_mono_obs.at(curr_time)->asVec() - Eigen::Vector2d(pf_cl.x()/pf_cl.z(), pf_cl.y()/pf_cl.z());\n        \n        Eigen::Matrix<double, 2, 3> H_proj = Eigen::Matrix<double, 2, 3>::Zero();\n        H_proj(0, 0) = 1.0/pf_cl.z();\n        H_proj(1, 1) = 1.0/pf_cl.z();\n        H_proj(0, 2) = -pf_cl.x()/std::pow(pf_cl.z(), 2);\n        H_proj(1, 2) = -pf_cl.y()/std::pow(pf_cl.z(), 2);\n        \n        Eigen::Matrix3d R_w2cl = R_cl2i_T*R_i2w_T;\n        \n        H_fj_epose.setZero();\n        H_fj_epose.block<2, 3>(0, 0) = H_proj*R_w2cl*skew(pf_w);\n        H_fj_epose.block<2, 3>(0, 3) = -H_proj*R_w2cl;\n        \n        H_fj_ext.setZero();\n        H_fj_ext.block<2, 3>(0, 0) = H_proj*R_cl2i_T*skew(pf_i);\n        H_fj_ext.block<2, 3>(0, 3) = -H_proj*R_cl2i_T;\n        \n        H_fj_anch.setZero();\n        H_fj_anch.block<2, 3>(0, 0) = -H_proj*R_w2cl*skew(pf_w);\n        \n        H_fj_pf = H_proj*R_w2cl;\n    }\n    \n    void LandmarkUpdate::calcResJacobianSingleLandmarkMono(\n        const std::shared_ptr<FeatureInfo> feature_info,\n        const std::shared_ptr<State> state,\n        Eigen::Vector2d& res,\n        Eigen::Matrix<double, 2, 6>& H_fj_pose,\n        Eigen::Matrix<double, 2, 6>& H_fj_anch,\n        Eigen::Matrix<double, 2, 3>& H_fj_pf)\n    {\n        const Eigen::Vector3d pf_w = feature_info->_landmark->valuePosXyz();\n        \n        const double curr_time = state->_timestamp;\n        \n        const Eigen::Matrix3d R_cm_T = state->_sw_camleft_poses.at(curr_time)->valueLinearAsMat().transpose();        \n        const Eigen::Vector3d p_cm = state->_sw_camleft_poses.at(curr_time)->valueTrans();\n        \n        const Eigen::Vector3d pf_cl = R_cm_T*(pf_w-p_cm);\n        \n        if (feature_info->_mono_obs.find(curr_time) == feature_info->_mono_obs.end() ||\n            feature_info->_ftype != FeatureInfo::FeatureType::SLAM)\n        {\n            std::cout << \"[LandmarkUpdate]: Cannot calc curr slam feature mono res and jacobi!\" << std::endl;\n            std::exit(EXIT_FAILURE);\n        }\n        \n        res = feature_info->_mono_obs.at(curr_time)->asVec() - Eigen::Vector2d(pf_cl.x()/pf_cl.z(), pf_cl.y()/pf_cl.z());\n        \n        Eigen::Matrix<double, 2, 3> H_proj = Eigen::Matrix<double, 2, 3>::Zero();\n        H_proj(0, 0) = 1.0/pf_cl.z();\n        H_proj(1, 1) = 1.0/pf_cl.z();\n        H_proj(0, 2) = -pf_cl.x()/std::pow(pf_cl.z(), 2);\n        H_proj(1, 2) = -pf_cl.y()/std::pow(pf_cl.z(), 2);\n        \n        H_fj_pose.setZero();\n        H_fj_anch.setZero();\n        \n        if (state->_sw_camleft_poses.at(curr_time) != feature_info->_landmark->getAnchoredPose())\n        {\n            H_fj_pose.block<2, 3>(0, 0) = H_proj*R_cm_T*skew(pf_w);\n            H_fj_anch.block<2, 3>(0, 0) = -H_fj_pose.block<2, 3>(0, 0);\n        }\n        \n        H_fj_pose.block<2, 3>(0, 3) = -H_proj*R_cm_T;\n        H_fj_pf = -H_fj_pose.block<2, 3>(0, 3);\n    }\n    \n    void LandmarkUpdate::calcResJacobianSingleLandmarkStereo(\n        const std::shared_ptr<FeatureInfo> feature_info,\n        const std::shared_ptr<State> state,\n        Eigen::Vector4d& res,\n        Eigen::Matrix<double, 4, 9>& H_fj_epose,\n        Eigen::Matrix<double, 4, 6>& H_fj_ext,\n        Eigen::Matrix<double, 4, 6>& H_fj_anch,\n        Eigen::Matrix<double, 4, 3>& H_fj_pf)\n    {\n        const Eigen::Vector3d pf_w = feature_info->_landmark->valuePosXyz();\n        \n        const Eigen::Matrix3d R_i2w_T = state->_extended_pose->valueLinearAsMat().transpose();\n        const Eigen::Matrix3d R_cl2i_T = state->_camleft_imu_extrinsics->valueLinearAsMat().transpose();\n        \n        const Eigen::Vector3d p_i2w = state->_extended_pose->valueTrans1();\n        const Eigen::Vector3d p_c2i = state->_camleft_imu_extrinsics->valueTrans();\n        \n        const Eigen::Vector3d pf_i = R_i2w_T*(pf_w-p_i2w);\n        const Eigen::Vector3d pf_cl = R_cl2i_T*(pf_i-p_c2i);\n        \n        const Eigen::Isometry3d& T_cl2cr = state->_state_params._T_cl2cr;\n        const Eigen::Vector3d pf_cr = T_cl2cr*pf_cl;\n        \n        const double curr_time = state->_timestamp;\n        \n        if (feature_info->_stereo_obs.find(curr_time) == feature_info->_stereo_obs.end() ||\n            feature_info->_ftype != FeatureInfo::FeatureType::SLAM)\n        {\n            std::cout << \"[LandmarkUpdate]: Cannot calc curr slam feature stereo res and jacobi!\" << std::endl;\n            std::exit(EXIT_FAILURE);\n        }\n        \n        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());\n        \n        Eigen::Matrix<double, 2, 3> H_proj = Eigen::Matrix<double, 2, 3>::Zero();\n        H_proj(0, 0) = 1.0/pf_cl.z();\n        H_proj(1, 1) = 1.0/pf_cl.z();\n        H_proj(0, 2) = -pf_cl.x()/std::pow(pf_cl.z(), 2);\n        H_proj(1, 2) = -pf_cl.y()/std::pow(pf_cl.z(), 2);\n        \n        Eigen::Matrix<double, 2, 3> H_proj_r = Eigen::Matrix<double, 2, 3>::Zero();\n        H_proj_r(0, 0) = 1.0/pf_cr.z();\n        H_proj_r(1, 1) = 1.0/pf_cr.z();\n        H_proj_r(0, 2) = -pf_cr.x()/std::pow(pf_cr.z(), 2);\n        H_proj_r(1, 2) = -pf_cr.y()/std::pow(pf_cr.z(), 2);\n        \n        Eigen::Matrix3d R_w2cl = R_cl2i_T*R_i2w_T;\n        \n        H_fj_epose.setZero();\n        H_fj_epose.block<2, 3>(0, 0) = H_proj*R_w2cl*skew(pf_w);\n        H_fj_epose.block<2, 3>(0, 3) = -H_proj*R_w2cl;\n        H_fj_epose.block<2, 3>(2, 0) = H_proj_r*T_cl2cr.linear()*R_w2cl*skew(pf_w);\n        H_fj_epose.block<2, 3>(2, 3) = -H_proj_r*T_cl2cr.linear()*R_w2cl;\n        \n        H_fj_ext.setZero();\n        H_fj_ext.block<2, 3>(0, 0) = H_proj*R_cl2i_T*skew(pf_i);\n        H_fj_ext.block<2, 3>(0, 3) = -H_proj*R_cl2i_T;\n        H_fj_ext.block<2, 3>(2, 0) = H_proj_r*T_cl2cr.linear()*R_cl2i_T*skew(pf_i);\n        H_fj_ext.block<2, 3>(2, 3) = -H_proj_r*T_cl2cr.linear()*R_cl2i_T;\n        \n        H_fj_anch.setZero();\n        H_fj_anch.block<2, 3>(0, 0) = -H_proj*R_w2cl*skew(pf_w);\n        H_fj_anch.block<2, 3>(2, 0) = -H_proj_r*T_cl2cr.linear()*skew(pf_w);\n        \n        H_fj_pf.setZero();\n        H_fj_pf.block<2, 3>(0, 0) = H_proj*R_w2cl;\n        H_fj_pf.block<2, 3>(2, 0) = H_proj_r*T_cl2cr.linear()*R_w2cl;\n    }\n    \n    void LandmarkUpdate::updateLandmarkStereo(std::shared_ptr<State> state,\n                                              std::shared_ptr<MapServer> map_server)\n    {\n        if (state->_anchored_landmarks.size() == 0) return;\n        \n        std::vector<std::shared_ptr<Type>> var_order;\n        var_order.push_back(state->_extended_pose);\n        var_order.push_back(state->_camleft_imu_extrinsics);\n        \n        std::map<std::shared_ptr<Type>, int> sub_var_col_idx;\n        sub_var_col_idx[state->_extended_pose] = 0;\n        sub_var_col_idx[state->_camleft_imu_extrinsics] = 9;\n        \n        const int max_possible_rows = 4*state->_anchored_landmarks.size();\n        const int max_possible_cols = 15 + 6*state->_sw_camleft_poses.size() + 3*state->_anchored_landmarks.size();\n        \n        Eigen::MatrixXd H_large(max_possible_rows, max_possible_cols);\n        Eigen::VectorXd res_large(max_possible_rows);\n        H_large.setZero();\n        res_large.setZero();\n        \n        int row_cnt = 0;\n        int col_cnt = 15;\n        \n        for (const auto& item : state->_anchored_landmarks)\n        {\n            const int& id = item.first;\n            if (map_server->find(id) == map_server->end())\n            {\n                std::cout << \"[LandmarkUpdate]: Landmark in state not in map server!\" << std::endl;\n                std::exit(EXIT_FAILURE);\n            }\n            else if (map_server->at(id)->_ftype != FeatureInfo::FeatureType::SLAM)\n            {\n                std::cout << \"[LandmarkUpdate]: Landmark in state not marked SLAM type in map server!\" << std::endl;\n                std::exit(EXIT_FAILURE);\n            }\n            else if (map_server->at(id)->_stereo_obs.find(state->_timestamp) == map_server->at(id)->_stereo_obs.end())\n            {\n                std::cout << \"[LandmarkUpdate]: Landmark in state not tracked to curr time! Should have been marged before!\" << std::endl;\n                std::exit(EXIT_FAILURE);\n            }\n            \n            if (map_server->at(id)->_landmark != state->_anchored_landmarks.at(id))\n            {\n                std::cout << \"[LandmarkUpdate]: Landmark ptr in state not the same as that in map server!\" << std::endl;\n                std::exit(EXIT_FAILURE);\n            }\n            \n            std::shared_ptr<SE3> anchored_pose_ptr = item.second->getAnchoredPose();\n            std::shared_ptr<AnchoredLandmark> lm_ptr = item.second;\n            \n            Eigen::Vector4d res;\n            Eigen::Matrix<double, 4, 9> H_fj_epose;\n            Eigen::Matrix<double, 4, 6> H_fj_ext;\n            Eigen::Matrix<double, 4, 6> H_fj_anch;\n            Eigen::Matrix<double, 4, 3> H_fj_pf;\n            \n            this->calcResJacobianSingleLandmarkStereo(map_server->at(id), state,\n                                                      res, H_fj_epose,\n                                                      H_fj_ext, H_fj_anch, H_fj_pf);\n            \n            std::vector<std::shared_ptr<Type>> var_order_chi2(4);\n            var_order_chi2[0] = state->_extended_pose;\n            var_order_chi2[1] = state->_camleft_imu_extrinsics;\n            var_order_chi2[2] = anchored_pose_ptr;\n            var_order_chi2[3] = lm_ptr;\n            \n            Eigen::MatrixXd H_chi2(4, 24);\n            H_chi2.block<4, 9>(0, 0) = H_fj_epose;\n            H_chi2.block<4, 6>(0, 9) = H_fj_ext;\n            H_chi2.block<4, 6>(0, 15) = H_fj_anch;\n            H_chi2.block<4, 3>(0, 21) = H_fj_pf;\n            \n            if (!testChiSquared(state, res, H_chi2, var_order_chi2, this->_noise))\n                continue;\n            \n            res_large.block<4, 1>(row_cnt, 0) = res;\n            \n            H_large.block<4, 9>(row_cnt, sub_var_col_idx.at(state->_extended_pose)) = H_fj_epose;\n            \n            H_large.block<4, 6>(row_cnt, sub_var_col_idx.at(state->_camleft_imu_extrinsics)) = H_fj_ext;\n            \n            if (sub_var_col_idx.find(anchored_pose_ptr) == sub_var_col_idx.end())\n            {\n                sub_var_col_idx[anchored_pose_ptr] = col_cnt;\n                col_cnt += 6;\n                var_order.push_back(anchored_pose_ptr);\n            }\n            \n            H_large.block<4, 6>(row_cnt, sub_var_col_idx.at(anchored_pose_ptr)) = H_fj_anch;\n            \n            if (sub_var_col_idx.find(lm_ptr) == sub_var_col_idx.end())\n            {\n                sub_var_col_idx[lm_ptr] = col_cnt;\n                col_cnt += 3;\n                var_order.push_back(lm_ptr);\n            }\n            \n            H_large.block<4, 3>(row_cnt, sub_var_col_idx.at(lm_ptr)) = H_fj_pf;\n            \n            row_cnt += 4;\n        }\n        \n        if (row_cnt == 0) return;\n        \n        if (max_possible_rows > row_cnt || max_possible_cols > col_cnt)\n        {\n            H_large.conservativeResize(row_cnt, col_cnt);\n            res_large.conservativeResize(row_cnt, 1);\n        }\n        \n        StateManager::ekfUpdate(state, var_order, H_large, res_large, std::pow(this->_noise, 2)*Eigen::MatrixXd::Identity(res_large.rows(), res_large.rows()));\n    }\n    \n    void LandmarkUpdate::calcResJacobianSingleFeatAllStereoObs(\n        const std::shared_ptr<FeatureInfo> feature_info,\n        const std::map<double, std::shared_ptr<SE3>>& sw_poses,\n        const std::vector<std::shared_ptr<SE3>>& sw_var_order,\n        const std::map<std::shared_ptr<SE3>, int>& sw_index_map,\n        const Eigen::Isometry3d& T_cl2cr,\n        Eigen::VectorXd& res_block,\n        Eigen::MatrixXd& Hx_block,\n        Eigen::MatrixXd& Hf_block)\n    {\n        const int num_of_cols = 6*sw_var_order.size();\n        const int num_of_rows = 4*feature_info->numOfStereoFrames();\n        \n        Eigen::VectorXd res_block_tmp = Eigen::VectorXd::Zero(num_of_rows);\n        Eigen::MatrixXd H_block_tmp = Eigen::MatrixXd::Zero(num_of_rows, num_of_cols);\n        Eigen::MatrixXd Hf_block_tmp = Eigen::MatrixXd::Zero(num_of_rows, 3);\n        \n        const Eigen::Vector3d pf_w = feature_info->_landmark->valuePosXyz();\n        \n        int row_cnt = 0;\n        \n        for (const auto& item : feature_info->_stereo_obs)\n        {\n            const double& time_of_obs = item.first;\n            \n            if (sw_poses.find(time_of_obs) == sw_poses.end())\n                continue;\n            \n            const std::shared_ptr<StereoMeas>& stereo_obs_ptr = item.second;\n            const std::shared_ptr<SE3>& pose_obs_ptr = sw_poses.at(time_of_obs);\n            const std::shared_ptr<SE3> pose_anchor_ptr = feature_info->_landmark->getAnchoredPose();\n            \n            const Eigen::Matrix3d R_cm2w = pose_obs_ptr->valueLinearAsMat();\n            const Eigen::Vector3d p_cm = pose_obs_ptr->valueTrans();\n            \n            const Eigen::Vector3d pf_cm = R_cm2w.transpose()*(pf_w-p_cm);\n            const Eigen::Vector3d pf_cm_r = T_cl2cr*pf_cm;\n            \n            Eigen::Matrix<double, 2, 3> H_proj = Eigen::Matrix<double, 2, 3>::Zero();\n            H_proj(0, 0) = 1.0/pf_cm.z();\n            H_proj(0, 2) = -pf_cm.x()/std::pow(pf_cm.z(), 2);\n            H_proj(1, 1) = 1.0/pf_cm.z();\n            H_proj(1, 2) = -pf_cm.y()/std::pow(pf_cm.z(), 2);\n            \n            Eigen::Matrix<double, 2, 3> H_proj_r = Eigen::Matrix<double, 2, 3>::Zero();\n            H_proj_r(0, 0) = 1.0/pf_cm_r.z();\n            H_proj_r(1, 1) = 1.0/pf_cm_r.z();\n            H_proj_r(0, 2) = -pf_cm_r.x()/std::pow(pf_cm_r.z(), 2);\n            H_proj_r(1, 2) = -pf_cm_r.y()/std::pow(pf_cm_r.z(), 2);\n            \n            Eigen::MatrixXd H_pf2x = Eigen::MatrixXd::Zero(3, num_of_cols);\n            \n            if (pose_anchor_ptr != pose_obs_ptr)\n            {\n                H_pf2x.block<3, 3>(0, sw_index_map.at(pose_obs_ptr)) = R_cm2w.transpose()*skew(pf_w);\n                \n                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));\n            }\n            \n            H_pf2x.block<3, 3>(0, sw_index_map.at(pose_obs_ptr)+3) = -R_cm2w.transpose();\n            \n            Eigen::Matrix3d H_pf2pf = R_cm2w.transpose();\n            \n            if (H_proj.hasNaN() || H_pf2x.hasNaN()) \n                continue;\n            \n            H_block_tmp.block(row_cnt, 0, 2, num_of_cols) = H_proj*H_pf2x;\n            H_block_tmp.block(row_cnt+2, 0, 2, num_of_cols) = H_proj_r*T_cl2cr.linear()*H_pf2x;\n            \n            Hf_block_tmp.block(row_cnt, 0, 2, 3) = H_proj*H_pf2pf;\n            Hf_block_tmp.block(row_cnt+2, 0, 2, 3) = H_proj_r*T_cl2cr.linear()*H_pf2pf;\n            \n            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());\n            \n            row_cnt += 4;\n        }\n        \n        if (row_cnt < res_block.rows())\n        {\n            res_block_tmp.conservativeResize(row_cnt, 1);\n            H_block_tmp.conservativeResize(row_cnt, num_of_cols);\n            Hf_block_tmp.conservativeResize(row_cnt, 3);\n        }\n        \n        res_block = res_block_tmp;\n        Hx_block = H_block_tmp;\n        Hf_block = Hf_block_tmp;\n    }\n    \n    void LandmarkUpdate::initNewLandmarkStereo(std::shared_ptr<State> state,\n                                               std::shared_ptr<MapServer> map_server,\n                                               std::shared_ptr<Triangulator> tri,\n                                               int min_init_poses)\n    {\n        if (state->_sw_camleft_poses.size() < min_init_poses)\n            return;\n        \n        const int vac_num_lm = state->_state_params._max_landmarks - state->_anchored_landmarks.size();\n        \n        if (vac_num_lm <= 0) return;\n        \n        std::vector<int> ids_to_init;\n        \n        for (const auto& item : *map_server)\n        {\n            if (ids_to_init.size() >= vac_num_lm) break;\n            \n            if (item.second->_stereo_obs.size() < min_init_poses || \n                item.second->_ftype == FeatureInfo::FeatureType::SLAM)\n                continue;\n            \n            if (!FeatureInfoManager::triangulateFeatureInfoStereo(item.second, tri, state))\n                continue;\n            \n            ids_to_init.push_back(item.first);\n        }\n        \n        if (ids_to_init.size() == 0) return;\n        \n        std::vector<std::shared_ptr<SE3>> sw_var_order;\n        std::map<std::shared_ptr<SE3>, int> sw_index_map;\n        std::vector<std::shared_ptr<Type>> sw_var_type;\n        \n        this->generateSwVarOrder(state, sw_var_order, sw_index_map, sw_var_type);\n        \n        for (const int& id : ids_to_init)\n        {\n            Eigen::VectorXd res_block;\n            Eigen::MatrixXd Hx_block;\n            Eigen::MatrixXd Hf_block;\n            \n            this->calcResJacobianSingleFeatAllStereoObs(map_server->at(id),\n                                                        state->_sw_camleft_poses,\n                                                        sw_var_order,\n                                                        sw_index_map,\n                                                        state->_state_params._T_cl2cr,\n                                                        res_block, Hx_block, Hf_block);\n            \n            if (!StateManager::addVariableDelayed(state, map_server->at(id)->_landmark,\n                sw_var_type, Hx_block, Hf_block, res_block,\n                this->_noise, 0.95, true))\n                continue;\n            \n            if (state->_anchored_landmarks.find(id) != state->_anchored_landmarks.end())\n            {\n                std::cout << \"[LandmarkUpdate]: The id intended to add already in state!\" << std::endl;\n                std::exit(EXIT_FAILURE);\n            }\n            \n            state->_anchored_landmarks[id] = map_server->at(id)->_landmark;\n            \n            map_server->at(id)->_ftype = FeatureInfo::FeatureType::SLAM;\n        }\n    }\n    \n}\n"
  },
  {
    "path": "ingvio_estimator/src/LandmarkUpdate.h",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#pragma once\n\n#include <memory>\n\n#include \"IngvioParams.h\"\n#include \"MapServer.h\"\n#include \"Update.h\"\n\nnamespace ingvio\n{\n    class State;\n    class Triangulator;\n    \n    class LandmarkUpdate : public UpdateBase\n    {\n    public:\n        \n        using UpdateBase::UpdateBase;\n        \n        LandmarkUpdate(const IngvioParams& filter_params) :\n        UpdateBase(filter_params._chi2_max_dof, filter_params._chi2_thres),\n        _noise(filter_params._visual_noise)\n        {}\n        \n        LandmarkUpdate(const LandmarkUpdate&) = delete;\n        LandmarkUpdate operator=(const LandmarkUpdate&) = delete;\n        \n        virtual ~LandmarkUpdate() {}\n        \n        void updateLandmarkMono(std::shared_ptr<State> state,\n                                std::shared_ptr<MapServer> map_server);\n        \n        void updateLandmarkMonoSw(std::shared_ptr<State> state,\n                                  std::shared_ptr<MapServer> map_server);\n        \n        void initNewLandmarkMono(std::shared_ptr<State> state,\n                                 std::shared_ptr<MapServer> map_server,\n                                 std::shared_ptr<Triangulator> tri,\n                                 int min_init_poses);\n        \n        void changeLandmarkAnchor(std::shared_ptr<State> state, \n                                  std::shared_ptr<MapServer> map_server);\n        \n        void changeLandmarkAnchor(std::shared_ptr<State> state, \n                                  std::shared_ptr<MapServer> map_server,\n                                  const std::vector<double>& marg_kfs);\n        \n        void updateLandmarkStereo(std::shared_ptr<State> state,\n                                  std::shared_ptr<MapServer> map_server);\n        \n        \n        void initNewLandmarkStereo(std::shared_ptr<State> state,\n                                   std::shared_ptr<MapServer> map_server,\n                                   std::shared_ptr<Triangulator> tri,\n                                   int min_init_poses);\n        \n    protected:\n        \n        double _noise;\n        \n        void calcResJacobianSingleLandmarkMono(const std::shared_ptr<FeatureInfo> feature_info,\n                                               const std::shared_ptr<State> state,\n                                               Eigen::Vector2d& res,\n                                               Eigen::Matrix<double, 2, 9>& H_fj_epose,\n                                               Eigen::Matrix<double, 2, 6>& H_fj_ext,\n                                               Eigen::Matrix<double, 2, 6>& H_fj_anch,\n                                               Eigen::Matrix<double, 2, 3>& H_fj_pf);\n        \n        void calcResJacobianSingleFeatAllMonoObs(\n            const std::shared_ptr<FeatureInfo> feature_info,\n            const std::map<double, std::shared_ptr<SE3>>& sw_poses,\n            const std::vector<std::shared_ptr<SE3>>& sw_var_order,\n            const std::map<std::shared_ptr<SE3>, int>& sw_index_map,\n            Eigen::VectorXd& res_block,\n            Eigen::MatrixXd& Hx_block,\n            Eigen::MatrixXd& Hf_block);\n        \n        void calcResJacobianSingleLandmarkMono(const std::shared_ptr<FeatureInfo> feature_info,\n                                               const std::shared_ptr<State> state,\n                                               Eigen::Vector2d& res,\n                                               Eigen::Matrix<double, 2, 6>& H_fj_pose,\n                                               Eigen::Matrix<double, 2, 6>& H_fj_anch,\n                                               Eigen::Matrix<double, 2, 3>& H_fj_pf);\n        \n        void generateSwVarOrder(const std::shared_ptr<State> state,\n                                std::vector<std::shared_ptr<SE3>>& sw_var_order,\n                                std::map<std::shared_ptr<SE3>, int>& sw_index,\n                                std::vector<std::shared_ptr<Type>>& sw_var_type);\n        \n        void calcResJacobianSingleLandmarkStereo(const std::shared_ptr<FeatureInfo> feature_info,\n                                                 const std::shared_ptr<State> state,\n                                                 Eigen::Vector4d& res,\n                                                 Eigen::Matrix<double, 4, 9>& H_fj_epose,\n                                                 Eigen::Matrix<double, 4, 6>& H_fj_ext,\n                                                 Eigen::Matrix<double, 4, 6>& H_fj_anch,\n                                                 Eigen::Matrix<double, 4, 3>& H_fj_pf);\n        \n        void calcResJacobianSingleFeatAllStereoObs(\n            const std::shared_ptr<FeatureInfo> feature_info,\n            const std::map<double, std::shared_ptr<SE3>>& sw_poses,\n            const std::vector<std::shared_ptr<SE3>>& sw_var_order,\n            const std::map<std::shared_ptr<SE3>, int>& sw_index_map,\n            const Eigen::Isometry3d& T_cl2cr,\n            Eigen::VectorXd& res_block,\n            Eigen::MatrixXd& Hx_block,\n            Eigen::MatrixXd& Hf_block);\n    };\n}\n"
  },
  {
    "path": "ingvio_estimator/src/MapServer.cpp",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#include \"MapServer.h\"\n\nnamespace ingvio\n{\n    void MonoMeas::setFromMonoMsg(const feature_tracker::MonoMeas::ConstPtr& mono_msg)\n    {\n        _id = mono_msg->id;\n        _u0 = mono_msg->u0;\n        _v0 = mono_msg->v0;\n    }\n    \n    void MonoMeas::setFromMonoMsg(const feature_tracker::MonoMeas& mono_msg)\n    {\n        _id = mono_msg.id;\n        _u0 = mono_msg.u0;\n        _v0 = mono_msg.v0;\n    }\n    \n    void StereoMeas::setFromStereoMsg(const feature_tracker::StereoMeas::ConstPtr& stereo_msg)\n    {\n        _id = stereo_msg->id;\n        _u0 = stereo_msg->u0;\n        _v0 = stereo_msg->v0;\n        _u1 = stereo_msg->u1;\n        _v1 = stereo_msg->v1;\n    }\n    \n    void StereoMeas::setFromStereoMsg(const feature_tracker::StereoMeas& stereo_msg)\n    {\n        _id = stereo_msg.id;\n        _u0 = stereo_msg.u0;\n        _v0 = stereo_msg.v0;\n        _u1 = stereo_msg.u1;\n        _v1 = stereo_msg.v1;\n    }\n    \n    bool FeatureInfo::hasMonoObsAt(double timestamp) const\n    {  \n        return (_mono_obs.find(timestamp) != _mono_obs.end());\n        \n    }\n    \n    Eigen::Vector2d FeatureInfo::monoMeasAt(double timestamp) const\n    {  \n        if (hasMonoObsAt(timestamp))\n            return _mono_obs.at(timestamp)->asVec();\n        else \n            return Eigen::Vector2d::Zero();\n    }\n    \n    bool FeatureInfo::hasStereoObsAt(double timestamp) const\n    {  \n        return (_stereo_obs.find(timestamp) != _stereo_obs.end());\n    }\n    \n    Eigen::Vector4d FeatureInfo::stereoMeasAt(double timestamp) const\n    {\n        if (hasStereoObsAt(timestamp))\n            return _stereo_obs.at(timestamp)->asVec();\n        else\n            return Eigen::Vector4d::Zero();\n    }\n}\n"
  },
  {
    "path": "ingvio_estimator/src/MapServer.h",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#pragma once\n\n#include <feature_tracker/MonoFrame.h>\n#include <feature_tracker/MonoMeas.h>\n\n#include <feature_tracker/StereoFrame.h>\n#include <feature_tracker/StereoMeas.h>\n\n#include \"AnchoredLandmark.h\"\n\nnamespace ingvio\n{\n    struct MonoMeas\n    {\n        int _id;\n        double _u0;\n        double _v0;\n        \n        Eigen::Vector2d asVec() const\n        {  return Eigen::Vector2d(_u0, _v0);  }\n        \n        void setFromMonoMsg(const feature_tracker::MonoMeas::ConstPtr& mono_msg);\n        \n        void setFromMonoMsg(const feature_tracker::MonoMeas& mono_msg);\n    };\n    \n    struct StereoMeas\n    {\n        int _id;\n        double _u0;\n        double _v0;\n        double _u1;\n        double _v1;\n        \n        Eigen::Vector2d asLeftVec() const\n        {  return Eigen::Vector2d(_u0, _v0);  }\n        \n        Eigen::Vector2d asRightVec() const\n        {  return Eigen::Vector2d(_u1, _v1);  }\n        \n        Eigen::Vector4d asVec() const\n        {  return Eigen::Vector4d(_u0, _v0, _u1, _v1);  }\n        \n        void setFromStereoMsg(const feature_tracker::StereoMeas::ConstPtr& stereo_msg);\n        \n        void setFromStereoMsg(const feature_tracker::StereoMeas& stereo_msg);\n    };\n    \n    class FeatureInfo\n    {\n    public:\n        enum FeatureType {MSCKF = 0, SLAM};\n        \n        FeatureInfo() : _id(-1), _ftype(MSCKF), _isToMarg(false), _isTri(false), _numOfTri(0) \n        {\n            _mono_obs.clear();\n            _stereo_obs.clear();\n            \n            _landmark = std::make_shared<AnchoredLandmark>();\n        }\n        \n        ~FeatureInfo() = default;\n                \n        const int& getId() const\n        {  return _id;  }\n\n        const FeatureType& getFeatureType() const\n        {  return _ftype;  }\n        \n        const bool& isToMarg() const\n        {  return _isToMarg;  }\n        \n        const bool& isTri() const\n        {  return _isTri;  }\n        \n        bool hasMonoObsAt(double timestamp) const;\n        \n        Eigen::Vector2d monoMeasAt(double timestamp) const;\n        \n        bool hasStereoObsAt(double timestamp) const;\n        \n        Eigen::Vector4d stereoMeasAt(double timestamp) const;\n        \n        int numOfMonoFrames() const\n        {  return _mono_obs.size();  }\n        \n        int numOfStereoFrames() const\n        {  return _stereo_obs.size();  }\n        \n        const std::shared_ptr<SE3> anchor() const\n        {  return _landmark->getAnchoredPose();  }\n        \n        const std::shared_ptr<AnchoredLandmark> landmark() const\n        {  return _landmark;  }\n     \n     /*\n     protected:\n        friend class FeatureInfoManager;\n        friend class MapServerManager;\n     */\n     \n        int _id;\n        FeatureType _ftype;\n        bool _isToMarg;\n        bool _isTri;\n        int _numOfTri;\n        \n        std::shared_ptr<AnchoredLandmark> _landmark;\n        \n        std::map<double, std::shared_ptr<MonoMeas>> _mono_obs;\n        std::map<double, std::shared_ptr<StereoMeas>> _stereo_obs;\n    };\n    \n    typedef std::map<int, std::shared_ptr<FeatureInfo>> MapServer;\n}\n"
  },
  {
    "path": "ingvio_estimator/src/MapServerManager.cpp",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#include <iostream>\n#include <map>\n#include <unordered_map>\n#include <memory>\n#include <cmath>\n\n#include \"State.h\"\n#include \"StateManager.h\"\n\n#include \"MapServer.h\"\n#include \"MapServerManager.h\"\n\n#include \"Triangulator.h\"\n\nnamespace ingvio\n{\n    std::shared_ptr<MonoMeas> MonoMeasManager::convertFromMsg(\n        const feature_tracker::MonoMeas::ConstPtr& mono_msg)\n    {\n        std::shared_ptr<MonoMeas> mono_meas = std::make_shared<MonoMeas>();\n        \n        mono_meas->setFromMonoMsg(mono_msg);\n        \n        return mono_meas;\n    }\n    \n    std::shared_ptr<MonoMeas> MonoMeasManager::convertFromMsg(\n        const feature_tracker::MonoMeas& mono_msg)\n    {\n        std::shared_ptr<MonoMeas> mono_meas = std::make_shared<MonoMeas>();\n        \n        mono_meas->setFromMonoMsg(mono_msg);\n        \n        return mono_meas;\n    }\n    \n    std::shared_ptr<MonoMeas> MonoMeasManager::random(int id)\n    {\n        std::shared_ptr<MonoMeas> mono_meas = std::make_shared<MonoMeas>();\n        \n        Eigen::Vector2d vec = Eigen::Vector2d::Random();\n        \n        mono_meas->_id = id;\n        mono_meas->_u0 = vec.x();\n        mono_meas->_v0 = vec.y();\n        \n        return mono_meas;\n    }\n    \n    std::shared_ptr<StereoMeas> StereoMeasManager::convertFromMsg(\n        const feature_tracker::StereoMeas::ConstPtr& stereo_msg)\n    {\n        std::shared_ptr<StereoMeas> stereo_meas = std::make_shared<StereoMeas>();\n        \n        stereo_meas->setFromStereoMsg(stereo_msg);\n        \n        return stereo_meas;\n    }\n    \n    std::shared_ptr<StereoMeas> StereoMeasManager::convertFromMsg(\n        const feature_tracker::StereoMeas& stereo_msg)\n    {\n        std::shared_ptr<StereoMeas> stereo_meas = std::make_shared<StereoMeas>();\n        \n        stereo_meas->setFromStereoMsg(stereo_msg);\n        \n        return stereo_meas;\n    }\n    \n    std::shared_ptr<StereoMeas> StereoMeasManager::random(int id)\n    {\n        std::shared_ptr<StereoMeas> stereo_meas = std::make_shared<StereoMeas>();\n        \n        Eigen::Vector4d vec = Eigen::Vector4d::Random();\n        \n        stereo_meas->_id = id;\n        stereo_meas->_u0 = vec.x();\n        stereo_meas->_v0 = vec.y();\n        stereo_meas->_u1 = vec.z();\n        stereo_meas->_v1 = vec.w();\n        \n        return stereo_meas;\n    }\n    \n    void FeatureInfoManager::collectMonoMeas(std::shared_ptr<FeatureInfo> feature_info,\n                                             std::shared_ptr<State> state,\n                                             std::shared_ptr<MonoMeas> mono_meas)\n    {\n        const double& timestamp = state->_timestamp;\n        \n        if (state->_sw_camleft_poses.find(timestamp) == state->_sw_camleft_poses.end())\n        {\n            std::cout << \"[FeatureInfoManager]: Meas timestamp not in sw!\" << std::endl;\n            assert(false);\n        }\n        \n        if (feature_info->_mono_obs.size() == 0)\n        {\n            feature_info->_mono_obs[timestamp] = mono_meas;\n            feature_info->_id = mono_meas->_id;\n            \n            feature_info->_ftype = FeatureInfo::FeatureType::MSCKF;\n            feature_info->_isToMarg = false;\n            feature_info->_isTri = false;\n            \n            feature_info->_landmark->resetAnchoredPose(state->_sw_camleft_poses.at(timestamp));\n        }\n        else\n        {\n            if (feature_info->hasMonoObsAt(timestamp))\n            {\n                std::cout << \"[FeatureInfoManager]: Meas timestamp already in mono obs, skip adding! \" << std::endl;\n                return;\n            }\n            \n            if (feature_info->_ftype == FeatureInfo::FeatureType::SLAM)\n                feature_info->_mono_obs.clear();\n                \n            feature_info->_mono_obs[timestamp] = mono_meas;\n            \n            assert(feature_info->_id == mono_meas->_id);\n            \n            feature_info->_isToMarg = false;\n        }\n    }\n    \n    void FeatureInfoManager::collectStereoMeas(std::shared_ptr<FeatureInfo> feature_info,\n                                               std::shared_ptr<State> state,\n                                               std::shared_ptr<StereoMeas> stereo_meas)\n    {\n        const double& timestamp = state->_timestamp;\n        \n        if (state->_sw_camleft_poses.find(timestamp) == state->_sw_camleft_poses.end())\n        {\n            std::cout << \"[FeatureInfoManager]: Meas timestamp not in sw!\" << std::endl;\n            assert(false);\n        }\n        \n        if (feature_info->_stereo_obs.size() == 0)\n        {\n            feature_info->_stereo_obs[timestamp] = stereo_meas;\n            feature_info->_id = stereo_meas->_id;\n            \n            feature_info->_ftype = FeatureInfo::FeatureType::MSCKF;\n            feature_info->_isToMarg = false;\n            feature_info->_isTri = false;\n            \n            feature_info->_landmark->resetAnchoredPose(state->_sw_camleft_poses.at(timestamp));\n        }\n        else\n        {\n            if (feature_info->hasStereoObsAt(timestamp))\n            {\n                std::cout << \"[FeatureInfoManager]: Meas timestamp already in stereo obs, skip adding! \" << std::endl;\n                return;\n            }\n            \n            if (feature_info->_ftype == FeatureInfo::FeatureType::SLAM)\n                feature_info->_stereo_obs.clear();\n            \n            feature_info->_stereo_obs[timestamp] = stereo_meas;\n            \n            assert(feature_info->_id == stereo_meas->_id);\n            \n            feature_info->_isToMarg = false;\n        }\n    }\n    \n    void MapServerManager::collectMonoMeas(std::shared_ptr<MapServer> map_server,\n                                           std::shared_ptr<State> state,\n                                           const feature_tracker::MonoFrame::ConstPtr& mono_frame_msg)\n    {\n        assert(mono_frame_msg->header.stamp.toSec() == state->_timestamp);\n        \n        for (const auto& item : mono_frame_msg->mono_features)\n        {\n            if (map_server->find(item.id) == map_server->end())\n                map_server->insert(std::make_pair(item.id, std::make_shared<FeatureInfo>()));\n                \n            FeatureInfoManager::collectMonoMeas(map_server->at(item.id), state, MonoMeasManager::convertFromMsg(item));\n        }\n    }\n    \n    void MapServerManager::collectStereoMeas(std::shared_ptr<MapServer> map_server,\n                                             std::shared_ptr<State> state,\n                                             const feature_tracker::StereoFrame::ConstPtr& stereo_frame_msg)\n    {\n        assert(stereo_frame_msg->header.stamp.toSec() == state->_timestamp);\n        \n        for (const auto& item : stereo_frame_msg->stereo_features)\n        {\n            if (map_server->find(item.id) == map_server->end())\n                map_server->insert(std::make_pair(item.id, std::make_shared<FeatureInfo>()));\n            \n            FeatureInfoManager::collectStereoMeas(map_server->at(item.id), state, StereoMeasManager::convertFromMsg(item));\n        }\n    }\n    \n    void MapServerManager::markMargMonoFeatures(std::shared_ptr<MapServer> map_server,\n                                                std::shared_ptr<State> state)\n    {\n        const double& curr_timestamp = state->_timestamp;\n        \n        std::vector<int> marg_ids;\n        \n        for (auto& item : *map_server)\n            if (!item.second->hasMonoObsAt(curr_timestamp))\n            {\n                item.second->_isToMarg = true;\n                    \n                if (item.second->_ftype == FeatureInfo::FeatureType::SLAM)\n                {\n                    assert(item.second->_id == item.first);\n                    marg_ids.push_back(item.second->_id);\n                }\n            }\n            \n        for (int i = 0; i < marg_ids.size(); ++i)\n        {\n            StateManager::margAnchoredLandmarkInState(state, marg_ids[i]);\n                \n            map_server->erase(marg_ids[i]);\n        }\n    }\n    \n    void MapServerManager::markMargStereoFeatures(std::shared_ptr<MapServer> map_server,\n                                                  std::shared_ptr<State> state)\n    {\n        const double& curr_timestamp = state->_timestamp;\n        \n        std::vector<int> marg_ids;\n        \n        for (auto& item : *map_server)\n        {\n            if (!item.second->hasStereoObsAt(curr_timestamp))\n            {\n                item.second->_isToMarg = true;\n                    \n                if (item.second->_ftype == FeatureInfo::FeatureType::SLAM)\n                {\n                    assert(item.second->_id == item.first);\n                    marg_ids.push_back(item.second->_id);\n                }\n            }\n        }\n            \n        for (int i = 0; i < marg_ids.size(); ++i)\n        {\n            StateManager::margAnchoredLandmarkInState(state, marg_ids[i]);\n                \n            map_server->erase(marg_ids[i]);\n        }\n    }\n    \n    bool FeatureInfoManager::triangulateFeatureInfoMono(\n        std::shared_ptr<FeatureInfo> feature_info,\n        const std::shared_ptr<Triangulator> tri,\n        const std::shared_ptr<State> state)\n    {\n        Eigen::Vector3d pf;\n        \n        bool flag = tri->triangulateMonoObs(feature_info->_mono_obs, state->_sw_camleft_poses, pf);\n        \n        if (flag && !pf.hasNaN())\n        {\n            ++feature_info->_numOfTri;\n            \n            Eigen::Vector3d body_anchor = feature_info->_landmark->getAnchoredPose()->copyValueAsIso().inverse()*pf;\n            \n            if (body_anchor.z() <= 0) return false;\n            \n            if (!feature_info->_isTri)\n            {\n                feature_info->_landmark->setFejPosXyz(pf);\n                feature_info->_landmark->setValuePosXyz(pf);\n                feature_info->_isTri = true;\n                \n                return true;\n            }\n                \n            feature_info->_landmark->setValuePosXyz(pf);\n                \n            return true;\n        }\n        else\n            return false;\n    }\n    \n    bool FeatureInfoManager::triangulateFeatureInfoStereo(\n        std::shared_ptr<FeatureInfo> feature_info,\n        const std::shared_ptr<Triangulator> tri,\n        const std::shared_ptr<State> state)\n    {\n        Eigen::Vector3d pf;\n        \n        bool flag = tri->triangulateStereoObs(feature_info->_stereo_obs, state->_sw_camleft_poses, state->_state_params._T_cl2cr, pf);\n        \n        if (flag && !pf.hasNaN())\n        {\n            ++feature_info->_numOfTri;\n            \n            Eigen::Vector3d body_anchor = feature_info->_landmark->getAnchoredPose()->copyValueAsIso().inverse()*pf;\n            \n            if (body_anchor.z() <= 0) return false;\n            \n            if (!feature_info->_isTri)\n            {\n                feature_info->_landmark->setFejPosXyz(pf);\n                feature_info->_landmark->setValuePosXyz(pf);\n                feature_info->_isTri = true;\n                \n                return true;\n            }\n            \n            feature_info->_landmark->setValuePosXyz(pf);\n            \n            return true;\n        }\n        else\n            return false;\n    }\n    \n    void FeatureInfoManager::changeAnchoredPose(std::shared_ptr<FeatureInfo> feature_info,\n                                                std::shared_ptr<State> state,\n                                                double target_sw_timestamp)\n    {\n        if (state->_sw_camleft_poses.size() < 2) return;\n        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())\n            return;\n        \n        if (feature_info->_ftype != FeatureInfo::FeatureType::SLAM) return;\n        \n        bool notFoundcurr = true;\n        for (const auto& item : state->_sw_camleft_poses)\n            if (item.second == feature_info->anchor())\n            {\n                notFoundcurr = false;\n                break;\n            }\n        \n        if (notFoundcurr || state->_anchored_landmarks.at(feature_info->getId()) != feature_info->landmark()) return;\n\n        std::vector<std::shared_ptr<Type>> var_order;\n        var_order.push_back(feature_info->anchor());\n        var_order.push_back(state->_sw_camleft_poses.at(target_sw_timestamp));\n        var_order.push_back(state->_anchored_landmarks.at(feature_info->getId()));\n        \n        Eigen::MatrixXd H = Eigen::MatrixXd::Zero(3, 15);\n        const Eigen::Vector3d& pf = feature_info->landmark()->valuePosXyz();\n        \n        H.block<3, 3>(0, 0) = -skew(pf);\n        H.block<3, 3>(0, 6) = skew(pf);\n        H.block<3, 3>(0, 12) = Eigen::Matrix3d::Identity();\n        \n        StateManager::replaceVarLinear(state, feature_info->_landmark, var_order, H);\n        \n        feature_info->_landmark->resetAnchoredPose(state->_sw_camleft_poses.at(target_sw_timestamp), true);\n        \n    }\n    \n    void FeatureInfoManager::changeAnchoredPose(std::shared_ptr<FeatureInfo> feature_info,\n                                                std::shared_ptr<State> state)\n    {\n        if (state->_sw_camleft_poses.size() < 2) return;\n        \n        FeatureInfoManager::changeAnchoredPose(feature_info, state, state->_sw_camleft_poses.rbegin()->first);\n    }\n    \n    void MapServerManager::mapStatistics(const std::shared_ptr<MapServer> map_server)\n    {\n        int num_of_msckf = 0;\n        int num_of_slam = 0;\n        int num_of_err = 0;\n        \n        for (const auto& item : *map_server)\n        {\n            if (item.second->_ftype ==  FeatureInfo::FeatureType::SLAM)\n                ++num_of_slam;\n            else if (item.second->_ftype == FeatureInfo::FeatureType::MSCKF)\n                ++num_of_msckf;\n            \n            if (item.second->_mono_obs.size() == 0 && item.second->_stereo_obs.size() == 0)\n                ++num_of_err;\n        }\n        \n        std::cout << \"[MapServerManager]: Num of msckf feats = \" << num_of_msckf << \" Num of slam feats = \" << num_of_slam << std::endl;\n        \n        if (num_of_err > 0)\n            std::cout << \"[MapServerManager]: Warning!! \" << num_of_err << \" feats in map server is null without obs!\" << std::endl;\n    }\n    \n    void MapServerManager::checkMapStateConsistent(const std::shared_ptr<MapServer> map_server,\n                                                   const std::shared_ptr<State> state)\n    {\n        std::vector<int> ids_slam;\n        \n        for (const auto& item : *map_server)\n            if (item.second->_ftype == FeatureInfo::FeatureType::SLAM)\n            {\n                const int& id = item.first;\n                \n                ids_slam.push_back(id);\n                \n                if (state->_anchored_landmarks.find(id) == state->_anchored_landmarks.end())\n                    std::cout << \"[MapServerManager]: Map server slam feat id = \" << id << \" not in the state! \" << std::endl;\n                else\n                {\n                    if (state->_anchored_landmarks.at(id) != item.second->_landmark)\n                        std::cout << \"[MapServerManager]: Map server slam feat id = \" << id << \" lm ptr not the same with that in state! \" << std::endl;\n                    \n                    if (state->_anchored_landmarks.at(id)->getAnchoredPose() != item.second->_landmark->getAnchoredPose())\n                        std::cout << \"[MapServerManager]: Map server slam feat id = \" << id << \" anchor pose not the same with that of in state! \" << std::endl;\n                }\n            }\n        \n        if (ids_slam.size() != state->_anchored_landmarks.size())\n            std::cout << \"[MapServerManager]: Some state lm are no longer in map server!\" << std::endl;\n    }\n    \n    void MapServerManager::checkAnchorStatus(const std::shared_ptr<MapServer> map_server,\n                                             const std::shared_ptr<State> state)\n    {\n        std::unordered_map<std::shared_ptr<SE3>, double> sw_poses;\n        for (const auto& item : state->_sw_camleft_poses)\n            sw_poses.insert(std::make_pair(item.second, item.first));\n        \n        for (const auto& item : *map_server)\n            if (sw_poses.find(item.second->_landmark->getAnchoredPose()) == sw_poses.end())\n            {\n                std::cout << \"[MapServerManager]: Anchor of Feat id = \" << item.first << \" not in the sw! \" << std::endl;\n            }\n    }\n    \n    void MapServerManager::eraseInvalidFeatures(std::shared_ptr<MapServer> map_server,\n                                                std::shared_ptr<State> state)\n    {\n        std::vector<int> ids_to_remove;\n        \n        for (const auto& item : *map_server)\n        {\n            const int& id = item.first;\n            const auto& feat_info_ptr = item.second;\n            \n            if (!feat_info_ptr->_isTri) continue;\n            \n            if (feat_info_ptr->_landmark->getAnchoredPose() == nullptr)\n            {\n                ids_to_remove.push_back(id);\n                continue;\n            }\n            \n            const auto anchor_ptr = feat_info_ptr->_landmark->getAnchoredPose();\n            \n            Eigen::Vector3d body = anchor_ptr->copyValueAsIso().inverse()*feat_info_ptr->_landmark->valuePosXyz();\n            \n            if (body.z() <= 0.2) \n                ids_to_remove.push_back(id);\n        }\n        \n        if (ids_to_remove.size() > 0)\n            std::cout << \"[MapServerManager]: Num of feats have negative depth in anchor pose = \" << ids_to_remove.size() << std::endl;\n        \n        for (const int& id : ids_to_remove)\n        {\n            if (map_server->at(id)->_ftype == FeatureInfo::FeatureType::SLAM)\n                StateManager::margAnchoredLandmarkInState(state, id);\n            \n            map_server->erase(id);\n        }\n    }\n}\n\n"
  },
  {
    "path": "ingvio_estimator/src/MapServerManager.h",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#pragma once\n\n#include <feature_tracker/MonoFrame.h>\n#include <feature_tracker/MonoMeas.h>\n\n#include <feature_tracker/StereoFrame.h>\n#include <feature_tracker/StereoMeas.h>\n\n#include <memory>\n\nnamespace ingvio\n{\n    struct MonoMeas;\n    struct StereoMeas;\n    class FeatureInfo;\n    \n    class State;\n    class Triangulator;\n    \n    class MonoMeasManager\n    {\n    private:\n        MonoMeasManager() {}\n        MonoMeasManager(const MonoMeasManager&) {}\n        MonoMeasManager operator=(const MonoMeasManager&) = delete;\n        \n    public:\n        static std::shared_ptr<MonoMeas> convertFromMsg(\n            const feature_tracker::MonoMeas::ConstPtr& mono_msg);\n        \n        static std::shared_ptr<MonoMeas> convertFromMsg(\n            const feature_tracker::MonoMeas& mono_msg);\n        \n        static std::shared_ptr<MonoMeas> random(int id);\n    };\n    \n    class StereoMeasManager\n    {\n    private:\n        StereoMeasManager() {}\n        StereoMeasManager(const StereoMeasManager&) {}\n        StereoMeasManager operator=(const StereoMeasManager&) = delete;\n   \n    public:\n        static std::shared_ptr<StereoMeas> convertFromMsg(const feature_tracker::StereoMeas::ConstPtr& stereo_msg);\n        \n        static std::shared_ptr<StereoMeas> convertFromMsg(\n            const feature_tracker::StereoMeas& stereo_msg);\n        \n        static std::shared_ptr<StereoMeas> random(int id);\n    };\n    \n    class FeatureInfoManager\n    {\n    private:\n        FeatureInfoManager () {}\n        FeatureInfoManager(const FeatureInfoManager&) {}\n        FeatureInfoManager operator=(const FeatureInfoManager&) = delete;\n    \n    public:\n        static void collectMonoMeas(std::shared_ptr<FeatureInfo> feature_info,\n                                    std::shared_ptr<State> state,\n                                    std::shared_ptr<MonoMeas> mono_meas);\n        \n        static void collectStereoMeas(std::shared_ptr<FeatureInfo> feature_info,\n                                      std::shared_ptr<State> state,\n                                      std::shared_ptr<StereoMeas> stereo_meas);\n        \n        static bool triangulateFeatureInfoMono(std::shared_ptr<FeatureInfo> feature_info,\n                                               const std::shared_ptr<Triangulator> tri,\n                                               const std::shared_ptr<State> state);\n        \n        static bool triangulateFeatureInfoStereo(std::shared_ptr<FeatureInfo> feature_info,\n                                                 const std::shared_ptr<Triangulator> tri,\n                                                 const std::shared_ptr<State> state);\n        \n        static void changeAnchoredPose(std::shared_ptr<FeatureInfo> feature_info,\n                                       std::shared_ptr<State> state,\n                                       double target_sw_timestamp);\n        \n        static void changeAnchoredPose(std::shared_ptr<FeatureInfo> feature_info,\n                                       std::shared_ptr<State> state);\n    };\n    \n    class MapServerManager\n    {\n    private:\n        MapServerManager () {}\n        MapServerManager(const MapServerManager&) {}\n        MapServerManager operator=(const MapServerManager&) = delete;\n        \n    public:     \n        \n        static void collectMonoMeas(std::shared_ptr<MapServer> map_server,\n                                    std::shared_ptr<State> state,\n                                    const feature_tracker::MonoFrame::ConstPtr& mono_frame_msg);\n        \n        static void collectStereoMeas(std::shared_ptr<MapServer> map_server,\n                                      std::shared_ptr<State> state,\n                                      const feature_tracker::StereoFrame::ConstPtr& stereo_frame_msg);\n        \n        static void markMargMonoFeatures(std::shared_ptr<MapServer> map_server,\n                                         std::shared_ptr<State> state);\n        \n        static void markMargStereoFeatures(std::shared_ptr<MapServer> map_server,\n                                           std::shared_ptr<State> state);\n        \n        static void mapStatistics(const std::shared_ptr<MapServer> map_server);\n        \n        static void checkMapStateConsistent(const std::shared_ptr<MapServer> map_server,\n                                            const std::shared_ptr<State> state);\n        \n        static void checkAnchorStatus(const std::shared_ptr<MapServer> map_server,\n                                      const std::shared_ptr<State> state);\n        \n        static void eraseInvalidFeatures(std::shared_ptr<MapServer> map_server,\n                                         std::shared_ptr<State> state);\n    };\n}\n"
  },
  {
    "path": "ingvio_estimator/src/PoseState.cpp",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#include \"PoseState.h\"\n\nnamespace ingvio\n{\n    void SO3::update(const Eigen::VectorXd& dx)\n    {\n        assert(dx.rows() >= this->size() + this->idx());\n        _rot = GammaFunc(dx.block<3, 1>(this->idx(), 0), 0)*_rot;\n        _q = Eigen::Quaterniond(_rot);\n    }\n    \n    void SO3::setIdentity()\n    {\n        _q.setIdentity();\n        _rot.setIdentity();\n    }\n    \n    void SO3::setRandom()\n    {\n        _q = Eigen::Quaterniond(Eigen::Vector4d::Random());\n        _q.normalize();\n        _rot = _q.toRotationMatrix();\n    }\n    \n    void SO3::setValueByQuat(const Eigen::Quaterniond& other_quat)\n    {\n        _q = other_quat.normalized();\n        _rot = _q.toRotationMatrix();\n    }\n    \n    void SO3::setValueByMat(const Eigen::Matrix3d& other_mat)\n    {\n        _rot = other_mat;\n        _q = Eigen::Quaterniond(_rot);\n    }\n    \n    void SO3::setFejByQuat(const Eigen::Quaterniond& other_quat)\n    {\n        _q_fej = other_quat.normalized();\n        _rot_fej = _q_fej.toRotationMatrix();\n    }\n    \n    void SO3::setFejByMat(const Eigen::Matrix3d& other_mat)\n    {\n        _rot_fej = other_mat;\n        _q_fej = Eigen::Quaterniond(_rot_fej);\n    }\n    \n    std::shared_ptr<SO3> SO3::clone()\n    {\n        auto tmp = std::shared_ptr<SO3>(new SO3());\n        \n        tmp->setFejByQuat(this->fejAsQuat());\n        tmp->setValueByQuat(this->valueAsQuat());\n        \n        return tmp;\n    }\n    \n    void SE3::update(const Eigen::VectorXd& dx)\n    {\n        assert(dx.rows() >= this->idx() + this->size());\n        Eigen::Matrix3d Gamma0 = GammaFunc(dx.block<3, 1>(this->idx(), 0), 0);\n        \n        _rot = Gamma0*_rot;\n        _q = Eigen::Quaterniond(_rot);\n        \n        _vec = Gamma0*_vec + GammaFunc(dx.block<3, 1>(this->idx(), 0), 1)*dx.block<3, 1>(this->idx()+3, 0);\n    }\n    \n    void SE3::setIdentity()\n    {\n        _q.setIdentity();\n        _rot.setIdentity();\n        _vec.setZero();\n    }\n    \n    void SE3::setRandom()\n    {\n        _q = Eigen::Quaterniond(Eigen::Vector4d::Random());\n        _q.normalize();\n        _rot = _q.toRotationMatrix();\n        _vec.setRandom();\n    }\n    \n    Eigen::Isometry3d SE3::copyValueAsIso() const\n    {\n        Eigen::Isometry3d result;\n        result.setIdentity();\n        result.linear() = _rot;\n        result.translation() = _vec;\n        \n        return result;\n    }\n    \n    Eigen::Isometry3d SE3::copyFejAsIso() const\n    {\n        Eigen::Isometry3d result;\n        result.setIdentity();\n        result.linear() = _rot_fej;\n        result.translation() = _vec_fej;\n        \n        return result;\n    }\n    \n    void SE3::setValueLinearByQuat(const Eigen::Quaterniond& other_quat)\n    {\n        _q = other_quat.normalized();\n        _rot = _q.toRotationMatrix();\n    }\n    \n    void SE3::setValueLinearByMat(const Eigen::Matrix3d& other_mat)\n    {\n        _rot = other_mat;\n        _q = Eigen::Quaterniond(_rot);\n    }\n    \n    void SE3::setValueByIso(const Eigen::Isometry3d& other_iso)\n    {\n        this->setValueLinearByMat(other_iso.linear());\n        this->setValueTrans(other_iso.translation());\n    }\n    \n    void SE3::setFejLinearByQuat(const Eigen::Quaterniond& other_quat)\n    {\n        _q_fej = other_quat.normalized();\n        _rot_fej = _q_fej.toRotationMatrix();\n    }\n    \n    void SE3::setFejLinearByMat(const Eigen::Matrix3d& other_mat)\n    {\n        _rot_fej = other_mat;\n        _q_fej = Eigen::Quaterniond(_rot_fej);\n    }\n    \n    void SE3::setFejByIso(const Eigen::Isometry3d& other_iso)\n    {\n        this->setFejLinearByMat(other_iso.linear());\n        this->setFejTrans(other_iso.translation());\n    }\n    \n    std::shared_ptr<SE3> SE3::clone()\n    {\n        auto tmp = std::shared_ptr<SE3>(new SE3());\n        \n        tmp->setValueLinearByQuat(this->valueLinearAsQuat());\n        tmp->setValueTrans(this->valueTrans());\n        \n        tmp->setFejLinearByQuat(this->fejLinearAsQuat());\n        tmp->setFejTrans(this->fejTrans());\n        \n        return tmp;\n    }\n    \n    void SE23::update(const Eigen::VectorXd& dx)\n    {\n        assert(dx.rows() >= this->idx() + this->size());\n        \n        Eigen::Matrix3d Gamma0 = GammaFunc(dx.block<3, 1>(this->idx(), 0), 0);\n        Eigen::Matrix3d Gamma1 = GammaFunc(dx.block<3, 1>(this->idx(), 0), 1);\n        \n        _rot = Gamma0*_rot;\n        _q = Eigen::Quaterniond(_rot);\n        \n        _vec1 = Gamma0*_vec1 + Gamma1*dx.block<3, 1>(this->idx()+3, 0);\n        _vec2 = Gamma0*_vec2 + Gamma1*dx.block<3, 1>(this->idx()+6, 0);\n    }\n    \n    void SE23::setIdentity()\n    {\n        _q.setIdentity();\n        _rot.setIdentity();\n        _vec1.setZero();\n        _vec2.setZero();\n    }\n    \n    void SE23::setRandom()\n    {\n        _q = Eigen::Quaterniond(Eigen::Vector4d::Random());\n        _q.normalize();\n        _rot = _q.toRotationMatrix();\n        _vec1.setRandom();\n        _vec2.setRandom();\n    }\n    \n    Eigen::Isometry3d SE23::copyValueAsIso() const\n    {\n        Eigen::Isometry3d result;\n        result.setIdentity();\n        result.linear() = _rot;\n        result.translation() = _vec1;\n        \n        return result;\n    }\n    \n    Eigen::Isometry3d SE23::copyFejAsIso() const\n    {\n        Eigen::Isometry3d result;\n        result.setIdentity();\n        result.linear() = _rot_fej;\n        result.translation() = _vec1_fej;\n        \n        return result;\n    }\n    \n    void SE23::setValueLinearByQuat(const Eigen::Quaterniond& other_quat)\n    {\n        _q = other_quat.normalized();\n        _rot = _q.toRotationMatrix();\n    }\n    \n    void SE23::setValueLinearByMat(const Eigen::Matrix3d& other_mat)\n    {\n        _rot = other_mat;\n        _q = Eigen::Quaterniond(_rot);\n    }\n    \n    void SE23::setFejLinearByQuat(const Eigen::Quaterniond& other_quat)\n    {\n        _q_fej = other_quat.normalized();\n        _rot_fej = _q_fej.toRotationMatrix();\n    }\n    \n    void SE23::setFejLinearByMat(const Eigen::Matrix3d& other_mat)\n    {\n        _rot_fej = other_mat;\n        _q_fej = Eigen::Quaterniond(_rot_fej);\n    }\n    \n    std::shared_ptr<SE23> SE23::clone()\n    {\n        auto tmp = std::shared_ptr<SE23>(new SE23());\n        \n        tmp->setValueLinearByQuat(this->valueLinearAsQuat());\n        tmp->setValueTrans1(this->valueTrans1());\n        tmp->setValueTrans2(this->valueTrans2());\n        \n        tmp->setFejLinearByQuat(this->fejLinearAsQuat());\n        tmp->setFejTrans1(this->fejTrans1());\n        tmp->setFejTrans2(this->fejTrans2());\n        \n        return tmp;\n    }\n}\n"
  },
  {
    "path": "ingvio_estimator/src/PoseState.h",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#pragma once\n\n#include <Eigen/Geometry>\n\n#include \"AuxGammaFunc.h\"\n#include \"VecState.h\"\n\nnamespace ingvio\n{\n    class SO3 : public Type\n    {\n    public:\n        SO3() : Type(3)\n        {\n            _q.setIdentity();\n            _q_fej.setIdentity();\n            _rot.setIdentity();\n            _rot_fej.setIdentity();\n        }\n        \n        ~SO3() {}\n        \n        void update(const Eigen::VectorXd& dx) override;\n        \n        void setIdentity() override;\n        \n        void setRandom() override;\n        \n        const Eigen::Quaterniond& valueAsQuat() const\n        { return _q; }\n        \n        const Eigen::Matrix3d& valueAsMat() const\n        { return _rot; }\n        \n        const Eigen::Quaterniond& fejAsQuat() const\n        { return _q_fej; }\n        \n        const Eigen::Matrix3d& fejAsMat() const\n        { return _rot_fej; }\n        \n        void setValueByQuat(const Eigen::Quaterniond& other_quat);\n        \n        void setValueByMat(const Eigen::Matrix3d& other_mat);\n        \n        void setFejByQuat(const Eigen::Quaterniond& other_quat);\n        \n        void setFejByMat(const Eigen::Matrix3d& other_mat);\n        \n        std::shared_ptr<SO3> clone();\n        \n    protected:\n        Eigen::Quaterniond _q;\n        Eigen::Matrix3d _rot;\n        \n        Eigen::Quaterniond _q_fej;\n        Eigen::Matrix3d _rot_fej;\n    };\n    \n    class SE3 : public Type\n    {\n    public:\n        SE3() : Type(6)\n        {\n            _q.setIdentity();\n            _q_fej = _q;\n            \n            _rot.setIdentity();\n            _rot_fej = _rot;\n            \n            _vec.setZero();\n            _vec_fej = _vec;\n        }\n        \n        ~SE3() {}\n        \n        void update(const Eigen::VectorXd& dx) override;\n        \n        void setIdentity() override;\n        \n        void setRandom() override;\n        \n        const Eigen::Quaterniond& valueLinearAsQuat() const\n        { return _q; }\n        \n        const Eigen::Matrix3d& valueLinearAsMat() const\n        { return _rot; }\n        \n        const Eigen::Vector3d& valueTrans() const\n        { return _vec; }\n        \n        Eigen::Isometry3d copyValueAsIso() const;\n        \n        const Eigen::Quaterniond& fejLinearAsQuat() const\n        { return _q_fej; }\n        \n        const Eigen::Matrix3d& fejLinearAsMat() const\n        { return _rot_fej; }\n        \n        const Eigen::Vector3d& fejTrans() const\n        { return _vec_fej; }\n        \n        Eigen::Isometry3d copyFejAsIso() const;\n        \n        void setValueLinearByQuat(const Eigen::Quaterniond& other_quat);\n        \n        void setValueLinearByMat(const Eigen::Matrix3d& other_mat);\n        \n        void setValueTrans(const Eigen::Vector3d& other_vec)\n        {  _vec = other_vec;  }\n        \n        void setValueByIso(const Eigen::Isometry3d& other_iso);\n        \n        void setFejLinearByQuat(const Eigen::Quaterniond& other_quat);\n        \n        void setFejLinearByMat(const Eigen::Matrix3d& other_mat);\n        \n        void setFejTrans(const Eigen::Vector3d& other_vec)\n        {  _vec_fej = other_vec;  }\n        \n        void setFejByIso(const Eigen::Isometry3d& other_iso);\n        \n        std::shared_ptr<SE3> clone();\n        \n    protected:\n        Eigen::Quaterniond _q;\n        Eigen::Matrix3d _rot;\n        \n        Eigen::Quaterniond _q_fej;\n        Eigen::Matrix3d _rot_fej;\n        \n        Eigen::Vector3d _vec;\n        Eigen::Vector3d _vec_fej;\n    };\n    \n    class SE23 : public Type\n    {\n    public:\n        SE23() : Type(9)\n        {\n            _q.setIdentity();\n            _q_fej = _q;\n            \n            _rot.setIdentity();\n            _rot_fej = _rot;\n            \n            _vec1.setZero();\n            _vec1_fej = _vec1;\n            \n            _vec2.setZero();\n            _vec2_fej = _vec2;\n        }\n        \n        ~SE23() {}\n        \n        void update(const Eigen::VectorXd& dx) override;\n        \n        void setIdentity() override;\n        \n        void setRandom() override;\n        \n        const Eigen::Quaterniond& valueLinearAsQuat() const\n        { return _q; }\n        \n        const Eigen::Matrix3d& valueLinearAsMat() const\n        { return _rot; }\n        \n        const Eigen::Vector3d& valueTrans1() const\n        { return _vec1; }\n        \n        const Eigen::Vector3d& valueTrans2() const\n        { return _vec2; }\n        \n        Eigen::Isometry3d copyValueAsIso() const;\n        \n        const Eigen::Quaterniond& fejLinearAsQuat() const\n        {  return _q_fej; }\n        \n        const Eigen::Matrix3d& fejLinearAsMat() const\n        {  return _rot_fej; }\n        \n        const Eigen::Vector3d& fejTrans1() const\n        {  return _vec1_fej; }\n        \n        const Eigen::Vector3d& fejTrans2() const\n        {  return _vec2_fej; }\n        \n        Eigen::Isometry3d copyFejAsIso() const;\n        \n        void setValueLinearByQuat(const Eigen::Quaterniond& other_quat);\n        \n        void setValueLinearByMat(const Eigen::Matrix3d& other_mat);\n        \n        void setValueTrans1(const Eigen::Vector3d& other_vec)\n        {  _vec1 = other_vec;  }\n        \n        void setValueTrans2(const Eigen::Vector3d& other_vec)\n        {  _vec2 = other_vec;  }\n        \n        void setFejLinearByQuat(const Eigen::Quaterniond& other_quat);\n        \n        void setFejLinearByMat(const Eigen::Matrix3d& other_mat);\n        \n        void setFejTrans1(const Eigen::Vector3d& other_vec)\n        {  _vec1_fej = other_vec;  }\n        \n        void setFejTrans2(const Eigen::Vector3d& other_vec)\n        {  _vec2_fej = other_vec;  }\n        \n        std::shared_ptr<SE23> clone();\n        \n    protected:\n        Eigen::Quaterniond _q;\n        Eigen::Quaterniond _q_fej;\n        \n        Eigen::Matrix3d _rot;\n        Eigen::Matrix3d _rot_fej;\n        \n        Eigen::Vector3d _vec1;\n        Eigen::Vector3d _vec1_fej;\n        \n        Eigen::Vector3d _vec2;\n        Eigen::Vector3d _vec2_fej;\n    };\n    \n}\n"
  },
  {
    "path": "ingvio_estimator/src/RemoveLostUpdate.cpp",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#include <Eigen/SVD>\n\n#include <Eigen/SparseCore>\n#include <Eigen/QR>\n#include <Eigen/SPQRSupport>\n\n#include \"IngvioParams.h\"\n\n#include \"StateManager.h\"\n\n#include \"MapServer.h\"\n#include \"MapServerManager.h\"\n\n#include \"Triangulator.h\"\n\n#include \"RemoveLostUpdate.h\"\n\nnamespace ingvio\n{\n    void RemoveLostUpdate::updateStateMono(std::shared_ptr<State> state,\n                                           std::shared_ptr<MapServer> map_server,\n                                           std::shared_ptr<Triangulator> tri)\n    {\n        MapServerManager::markMargMonoFeatures(map_server, state);\n        \n        std::vector<int> update_ids, direct_marg_ids;\n        \n        for (auto& item : *map_server)\n            if (item.second->_ftype == FeatureInfo::FeatureType::MSCKF && item.second->_isToMarg)\n            {\n                if (FeatureInfoManager::triangulateFeatureInfoMono(item.second, tri, state) && item.second->_mono_obs.size() >= 4)\n                    update_ids.push_back(item.first);\n                else\n                    direct_marg_ids.push_back(item.first);\n            }    \n        \n        for (const auto& item : direct_marg_ids)\n            map_server->erase(item);\n        \n        if (update_ids.size() == 0) return;\n        \n        std::map<std::shared_ptr<Type>, int> sw_index_map;\n        std::vector<std::shared_ptr<Type>> sw_var_order;\n        \n        const int max_possible_cols = 6*state->_sw_camleft_poses.size();\n        \n        int row_cnt = 0;\n        for (int i = 0; i < update_ids.size(); ++i)\n            row_cnt += 2*map_server->at(update_ids[i])->numOfMonoFrames()-3;\n        const int max_possible_rows = row_cnt;\n        \n        Eigen::VectorXd res_large(max_possible_rows);\n        Eigen::MatrixXd H_large(max_possible_rows, max_possible_cols);\n        res_large.setZero();\n        H_large.setZero();\n        \n        int valid_id_cnt = 0;\n        \n        row_cnt = 0;\n        int col_cnt = 0;\n        \n        for (int i = 0; i < update_ids.size(); ++i)\n        {\n            Eigen::VectorXd res_block;\n            Eigen::MatrixXd H_block;\n            \n            std::map<std::shared_ptr<Type>, int> block_index_map;\n            std::vector<std::shared_ptr<Type>> block_var_order;\n            \n            this->calcResJacobianSingleFeatAllMonoObs(map_server->at(update_ids[i]),\n                                                      state->_sw_camleft_poses,\n                                                      block_var_order, block_index_map,\n                                                      res_block, H_block);\n            \n            if (!testChiSquared(state, res_block, H_block, block_var_order,\n                this->_noise, map_server->at(update_ids[i])->_mono_obs.size()-1))\n                continue;\n            \n            Eigen::MatrixXd H_large_row_block = Eigen::MatrixXd::Zero(res_block.rows(), max_possible_cols);\n            \n            for (const auto& item : block_index_map)\n            {\n                const std::shared_ptr<Type>& pose_ptr = item.first;\n                const int& sub_idx = item.second;\n                \n                if (sw_index_map.find(pose_ptr) == sw_index_map.end())\n                {\n                    sw_index_map[pose_ptr] = col_cnt;\n                    sw_var_order.push_back(pose_ptr);\n                    col_cnt += pose_ptr->size();\n                }\n                \n                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());\n            }\n            \n            H_large.block(row_cnt, 0, H_block.rows(), max_possible_cols) = H_large_row_block;\n            res_large.block(row_cnt, 0, res_block.rows(), 1) = res_block;\n            \n            row_cnt += res_block.rows();\n            ++valid_id_cnt;\n            \n            if (valid_id_cnt >= _max_valid_ids) break;\n        }\n        \n        /*\n        if (valid_id_cnt > 0)\n            std::cout << \"[RemoveLostUpdate]: Features used in remove update = \" << valid_id_cnt << std::endl;\n        */\n        \n        if (row_cnt < max_possible_rows || col_cnt < max_possible_cols)\n        {\n            H_large.conservativeResize(row_cnt, col_cnt);\n            res_large.conservativeResize(row_cnt, 1);\n        }\n        \n        Eigen::MatrixXd H_thin;\n        Eigen::VectorXd res_thin;\n        \n        if (H_large.rows() > H_large.cols())\n        {\n            Eigen::SparseMatrix<double> H_sparse = H_large.sparseView();\n            \n            Eigen::SPQR<Eigen::SparseMatrix<double>> spqr_helper;\n            spqr_helper.setSPQROrdering(SPQR_ORDERING_NATURAL);\n            spqr_helper.compute(H_sparse);\n            \n            Eigen::MatrixXd H_temp;\n            Eigen::VectorXd r_temp;\n            \n            (spqr_helper.matrixQ().transpose()*H_large).evalTo(H_temp);\n            (spqr_helper.matrixQ().transpose()*res_large).evalTo(r_temp);\n            \n            H_thin = H_temp.topRows(row_cnt);\n            res_thin = r_temp.head(row_cnt);                 \n        }\n        else\n        {\n            H_thin = H_large;\n            res_thin = res_large;\n        }\n        \n        if (res_thin.rows() > 0)\n            StateManager::ekfUpdate(state, sw_var_order, H_thin, res_thin, std::pow(this->_noise, 2)*Eigen::MatrixXd::Identity(res_thin.rows(), res_thin.rows()));\n        \n        for (const auto& item : update_ids)\n            map_server->erase(item);\n    }\n    \n    void RemoveLostUpdate::calcResJacobianSingleFeatAllMonoObs(\n        const std::shared_ptr<FeatureInfo> feature_info, \n        const std::map<double, std::shared_ptr<SE3>>& sw_poses,\n        std::vector<std::shared_ptr<Type>>& block_var_order,\n        std::map<std::shared_ptr<Type>, int>& block_index_map,\n        Eigen::VectorXd& res_block,\n        Eigen::MatrixXd& H_block)\n    {\n        const int max_possible_rows = 2*feature_info->numOfMonoFrames();\n        const int max_possible_cols = 6*sw_poses.size();\n        \n        Eigen::VectorXd res_block_tmp = Eigen::VectorXd::Zero(max_possible_rows);\n        Eigen::MatrixXd H_block_tmp = Eigen::MatrixXd::Zero(max_possible_rows, max_possible_cols);\n        Eigen::MatrixXd Hf_block_tmp = Eigen::MatrixXd::Zero(max_possible_rows, 3);\n        \n        const Eigen::Vector3d pf_w = feature_info->_landmark->valuePosXyz();\n        const std::shared_ptr<SE3> pose_anchor_ptr = feature_info->_landmark->getAnchoredPose();\n        \n        block_var_order.clear();\n        block_index_map.clear();\n        \n        block_index_map[pose_anchor_ptr] = 0;\n        block_var_order.push_back(pose_anchor_ptr);\n        \n        int row_cnt = 0;\n        int col_cnt = 6;\n        \n        for (const auto& item : feature_info->_mono_obs)\n        {\n            const double& time_of_obs = item.first;\n            \n            if (sw_poses.find(time_of_obs) == sw_poses.end())\n                continue;\n            \n            const std::shared_ptr<MonoMeas>& mono_obs_ptr = item.second;\n            const std::shared_ptr<SE3>& pose_obs_ptr = sw_poses.at(time_of_obs);\n            \n            const Eigen::Matrix3d R_cm2w = pose_obs_ptr->valueLinearAsMat();\n            const Eigen::Vector3d p_cm = pose_obs_ptr->valueTrans();\n            \n            const Eigen::Vector3d pf_cm = R_cm2w.transpose()*(pf_w-p_cm);\n            \n            Eigen::Matrix<double, 2, 3> H_proj = Eigen::Matrix<double, 2, 3>::Zero();\n            H_proj(0, 0) = 1.0/pf_cm.z();\n            H_proj(0, 2) = -pf_cm.x()/std::pow(pf_cm.z(), 2);\n            H_proj(1, 1) = 1.0/pf_cm.z();\n            H_proj(1, 2) = -pf_cm.y()/std::pow(pf_cm.z(), 2);\n            \n            bool flag = false;\n            \n            if (block_index_map.find(pose_obs_ptr) == block_index_map.end())\n            {\n                flag = true;\n                block_index_map[pose_obs_ptr] = col_cnt;\n                block_var_order.push_back(pose_obs_ptr);\n                col_cnt += pose_obs_ptr->size();\n            }\n            \n            Eigen::MatrixXd H_pf2x = Eigen::MatrixXd::Zero(3, max_possible_cols);\n            \n            if (pose_obs_ptr != pose_anchor_ptr)\n            {\n                H_pf2x.block<3, 3>(0, block_index_map.at(pose_obs_ptr)) = R_cm2w.transpose()*skew(pf_w);\n                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));\n            }\n             \n            H_pf2x.block<3, 3>(0, block_index_map.at(pose_obs_ptr)+3) = -R_cm2w.transpose();\n                \n            Eigen::Matrix3d H_pf2pf = R_cm2w.transpose();\n\n            if (H_proj.hasNaN() || H_pf2x.hasNaN()) \n            {\n                if (flag)\n                {\n                    block_index_map.erase(pose_obs_ptr);\n                    block_var_order.pop_back();\n                    col_cnt -= pose_obs_ptr->size();\n                }\n                continue;\n            }\n            \n            H_block_tmp.block(row_cnt, 0, 2, max_possible_cols) = H_proj*H_pf2x;\n            Hf_block_tmp.block(row_cnt, 0, 2, 3) = H_proj*H_pf2pf;\n            \n            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());\n            \n            row_cnt += 2;\n        }\n        \n        if (row_cnt < max_possible_rows || col_cnt < max_possible_cols)\n        {\n            res_block_tmp.conservativeResize(row_cnt, 1);\n            H_block_tmp.conservativeResize(row_cnt, col_cnt);\n            Hf_block_tmp.conservativeResize(row_cnt, 3);\n        }\n        \n        if (row_cnt <= 3)\n            std::cout << \"[RemoveLostUpdate]: Warning in mono feats removal, row cnt <= 3! \" << std::endl;\n        \n        Eigen::JacobiSVD<Eigen::MatrixXd> svd_helper(Hf_block_tmp, Eigen::ComputeFullU | Eigen::ComputeThinV);\n        Eigen::MatrixXd V = svd_helper.matrixU().rightCols(row_cnt-3);\n        \n        H_block = V.transpose()*H_block_tmp;\n        res_block = V.transpose()*res_block_tmp;\n    }\n    \n    \n    void RemoveLostUpdate::updateStateStereo(std::shared_ptr<State> state,\n                                             std::shared_ptr<MapServer> map_server,\n                                             std::shared_ptr<Triangulator> tri)\n    {\n        MapServerManager::markMargStereoFeatures(map_server, state);\n        \n        std::vector<int> update_ids, direct_marg_ids;\n        \n        for (auto& item : *map_server)\n            if (item.second->_ftype == FeatureInfo::FeatureType::MSCKF && item.second->_isToMarg)\n            {\n                if (FeatureInfoManager::triangulateFeatureInfoStereo(item.second, tri, state) && item.second->numOfStereoFrames() >= 3)\n                    update_ids.push_back(item.first);\n                else\n                    direct_marg_ids.push_back(item.first);\n            }    \n            \n        for (const auto& item : direct_marg_ids)\n                map_server->erase(item);\n            \n        if (update_ids.size() == 0) return;\n        \n        std::map<std::shared_ptr<Type>, int> sw_index_map;\n        std::vector<std::shared_ptr<Type>> sw_var_order;\n        \n        const int max_possible_cols = 6*state->_sw_camleft_poses.size();\n        \n        int row_cnt = 0;\n        for (int i = 0; i < update_ids.size(); ++i)\n            row_cnt += 4*map_server->at(update_ids[i])->numOfStereoFrames()-3;\n        const int max_possible_rows = row_cnt;\n        \n        Eigen::VectorXd res_large(max_possible_rows);\n        Eigen::MatrixXd H_large(max_possible_rows, max_possible_cols);\n        res_large.setZero();\n        H_large.setZero();\n        \n        int valid_id_cnt = 0;\n        \n        row_cnt = 0;\n        int col_cnt = 0;\n        \n        for (int i = 0; i < update_ids.size(); ++i)\n        {\n            Eigen::VectorXd res_block;\n            Eigen::MatrixXd H_block;\n            \n            std::map<std::shared_ptr<Type>, int> block_index_map;\n            std::vector<std::shared_ptr<Type>> block_var_order;\n            \n            this->calcResJacobianSingleFeatAllStereoObs(map_server->at(update_ids[i]),\n                                                        state->_sw_camleft_poses,\n                                                        state->_state_params._T_cl2cr,\n                                                        block_var_order, block_index_map,\n                                                        res_block, H_block);\n            \n            if (!testChiSquared(state, res_block, H_block, block_var_order,\n                this->_noise, map_server->at(update_ids[i])->_stereo_obs.size()-1))\n                continue;\n            \n            Eigen::MatrixXd H_large_row_block = Eigen::MatrixXd::Zero(res_block.rows(), max_possible_cols);\n            \n            for (const auto& item : block_index_map)\n            {\n                const std::shared_ptr<Type>& pose_ptr = item.first;\n                const int& sub_idx = item.second;\n                \n                if (sw_index_map.find(pose_ptr) == sw_index_map.end())\n                {\n                    sw_index_map[pose_ptr] = col_cnt;\n                    sw_var_order.push_back(pose_ptr);\n                    col_cnt += pose_ptr->size();\n                }\n                \n                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());\n            }\n            \n            H_large.block(row_cnt, 0, H_block.rows(), max_possible_cols) = H_large_row_block;\n            res_large.block(row_cnt, 0, res_block.rows(), 1) = res_block;\n            \n            row_cnt += res_block.rows();\n            ++valid_id_cnt;\n            \n            if (valid_id_cnt >= _max_valid_ids) break;\n        }\n        \n        /*\n         if (valid_id_cnt > 0)\n         std::cout << \"[RemoveLostUpdate]: Features used in remove update = \" << valid_id_cnt << std::endl;\n         */\n        \n        if (row_cnt < max_possible_rows || col_cnt < max_possible_cols)\n        {\n            H_large.conservativeResize(row_cnt, col_cnt);\n            res_large.conservativeResize(row_cnt, 1);\n        }\n        \n        Eigen::MatrixXd H_thin;\n        Eigen::VectorXd res_thin;\n        \n        if (H_large.rows() > H_large.cols())\n        {\n            Eigen::SparseMatrix<double> H_sparse = H_large.sparseView();\n            \n            Eigen::SPQR<Eigen::SparseMatrix<double>> spqr_helper;\n            spqr_helper.setSPQROrdering(SPQR_ORDERING_NATURAL);\n            spqr_helper.compute(H_sparse);\n            \n            Eigen::MatrixXd H_temp;\n            Eigen::VectorXd r_temp;\n            \n            (spqr_helper.matrixQ().transpose()*H_large).evalTo(H_temp);\n            (spqr_helper.matrixQ().transpose()*res_large).evalTo(r_temp);\n            \n            H_thin = H_temp.topRows(row_cnt);\n            res_thin = r_temp.head(row_cnt);                 \n        }\n        else\n        {\n            H_thin = H_large;\n            res_thin = res_large;\n        }\n        \n        if (res_thin.rows() > 0)\n            StateManager::ekfUpdate(state, sw_var_order, H_thin, res_thin, std::pow(this->_noise, 2)*Eigen::MatrixXd::Identity(res_thin.rows(), res_thin.rows()));\n        \n        for (const auto& item : update_ids)\n            map_server->erase(item);\n\n    }\n    \n    void RemoveLostUpdate::calcResJacobianSingleFeatAllStereoObs(\n        const std::shared_ptr<FeatureInfo> feature_info, \n        const std::map<double, std::shared_ptr<SE3>>& sw_poses,\n        const Eigen::Isometry3d& T_cl2cr,\n        std::vector<std::shared_ptr<Type>>& block_var_order,\n        std::map<std::shared_ptr<Type>, int>& block_index_map,\n        Eigen::VectorXd& res_block,\n        Eigen::MatrixXd& H_block)\n    {\n        const int max_possible_rows = 4*feature_info->numOfStereoFrames();\n        const int max_possible_cols = 6*sw_poses.size();\n        \n        Eigen::VectorXd res_block_tmp = Eigen::VectorXd::Zero(max_possible_rows);\n        Eigen::MatrixXd H_block_tmp = Eigen::MatrixXd::Zero(max_possible_rows, max_possible_cols);\n        Eigen::MatrixXd Hf_block_tmp = Eigen::MatrixXd::Zero(max_possible_rows, 3);\n        \n        const Eigen::Vector3d pf_w = feature_info->_landmark->valuePosXyz();\n        const std::shared_ptr<SE3> pose_anchor_ptr = feature_info->_landmark->getAnchoredPose();\n        \n        block_var_order.clear();\n        block_index_map.clear();\n        \n        block_index_map[pose_anchor_ptr] = 0;\n        block_var_order.push_back(pose_anchor_ptr);\n        \n        int row_cnt = 0;\n        int col_cnt = 6;\n        \n        for (const auto& item : feature_info->_stereo_obs)\n        {\n            const double& time_of_obs = item.first;\n            \n            if (sw_poses.find(time_of_obs) == sw_poses.end())\n                continue;\n            \n            const std::shared_ptr<StereoMeas>& stereo_obs_ptr = item.second;\n            const std::shared_ptr<SE3>& pose_obs_ptr = sw_poses.at(time_of_obs);\n            \n            const Eigen::Matrix3d R_cm2w = pose_obs_ptr->valueLinearAsMat();\n            const Eigen::Vector3d p_cm = pose_obs_ptr->valueTrans();\n            \n            const Eigen::Vector3d pf_cm = R_cm2w.transpose()*(pf_w-p_cm);\n            \n            const Eigen::Vector3d pf_cm_r = T_cl2cr*pf_cm;\n            \n            Eigen::Matrix<double, 2, 3> H_proj = Eigen::Matrix<double, 2, 3>::Zero();\n            H_proj(0, 0) = 1.0/pf_cm.z();\n            H_proj(0, 2) = -pf_cm.x()/std::pow(pf_cm.z(), 2);\n            H_proj(1, 1) = 1.0/pf_cm.z();\n            H_proj(1, 2) = -pf_cm.y()/std::pow(pf_cm.z(), 2);\n            \n            Eigen::Matrix<double, 2, 3> H_proj_r = Eigen::Matrix<double, 2, 3>::Zero();\n            H_proj_r(0, 0) = 1.0/pf_cm_r.z();\n            H_proj_r(0, 2) = -pf_cm_r.x()/std::pow(pf_cm_r.z(), 2);\n            H_proj_r(1, 1) = 1.0/pf_cm_r.z();\n            H_proj_r(1, 2) = -pf_cm_r.y()/std::pow(pf_cm_r.z(), 2);\n            \n            bool flag = false;\n            \n            if (block_index_map.find(pose_obs_ptr) == block_index_map.end())\n            {\n                flag = true;\n                block_index_map[pose_obs_ptr] = col_cnt;\n                block_var_order.push_back(pose_obs_ptr);\n                col_cnt += pose_obs_ptr->size();\n            }\n            \n            Eigen::MatrixXd H_pf2x = Eigen::MatrixXd::Zero(3, max_possible_cols);\n            \n            if (pose_obs_ptr != pose_anchor_ptr)\n            {\n                H_pf2x.block<3, 3>(0, block_index_map.at(pose_obs_ptr)) = R_cm2w.transpose()*skew(pf_w);\n                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));\n            }\n            \n            H_pf2x.block<3, 3>(0, block_index_map.at(pose_obs_ptr)+3) = -R_cm2w.transpose();\n            \n            Eigen::Matrix3d H_pf2pf = R_cm2w.transpose();\n            \n            if (H_proj.hasNaN() || H_pf2x.hasNaN()) \n            {\n                if (flag)\n                {\n                    block_index_map.erase(pose_obs_ptr);\n                    block_var_order.pop_back();\n                    col_cnt -= pose_obs_ptr->size();\n                }\n                continue;\n            }\n            \n            H_block_tmp.block(row_cnt, 0, 2, max_possible_cols) = H_proj*H_pf2x;\n            Hf_block_tmp.block(row_cnt, 0, 2, 3) = H_proj*H_pf2pf;\n            \n            H_block_tmp.block(row_cnt+2, 0, 2, max_possible_cols) = H_proj_r*T_cl2cr.linear()*H_pf2x;\n            Hf_block_tmp.block(row_cnt+2, 0, 2, 3) = H_proj_r*T_cl2cr.linear()*H_pf2pf;\n            \n            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());\n            \n            row_cnt += 4;\n        }\n        \n        if (row_cnt < max_possible_rows || col_cnt < max_possible_cols)\n        {\n            res_block_tmp.conservativeResize(row_cnt, 1);\n            H_block_tmp.conservativeResize(row_cnt, col_cnt);\n            Hf_block_tmp.conservativeResize(row_cnt, 3);\n        }\n        \n        if (row_cnt <= 3)\n            std::cout << \"[RemoveLostUpdate]: Warning in stereo feats removal, row cnt <= 3! \" << std::endl;\n        \n        Eigen::JacobiSVD<Eigen::MatrixXd> svd_helper(Hf_block_tmp, Eigen::ComputeFullU | Eigen::ComputeThinV);\n        Eigen::MatrixXd V = svd_helper.matrixU().rightCols(row_cnt-3);\n        \n        H_block = V.transpose()*H_block_tmp;\n        res_block = V.transpose()*res_block_tmp;\n    }\n}\n"
  },
  {
    "path": "ingvio_estimator/src/RemoveLostUpdate.h",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#pragma once\n\n#include \"MapServer.h\"\n\n#include \"Update.h\"\n\nnamespace ingvio\n{\n    class Triangulator;\n    class IngvioParams;\n    \n    class RemoveLostUpdate : public UpdateBase\n    {\n    public:\n        using UpdateBase::UpdateBase;\n        \n        RemoveLostUpdate(const IngvioParams& filter_params) : UpdateBase(filter_params._chi2_max_dof, filter_params._chi2_thres),\n        _max_valid_ids(20),\n        _noise(filter_params._visual_noise)\n        {}\n        \n        RemoveLostUpdate(const RemoveLostUpdate&) = delete;\n        RemoveLostUpdate operator=(const RemoveLostUpdate&) = delete;\n        \n        virtual ~RemoveLostUpdate() {}\n        \n        void updateStateMono(std::shared_ptr<State> state,\n                             std::shared_ptr<MapServer> map_server,\n                             std::shared_ptr<Triangulator> tri);\n        \n        void updateStateStereo(std::shared_ptr<State> state,\n                               std::shared_ptr<MapServer> map_server,\n                               std::shared_ptr<Triangulator> tri);\n        \n    protected:\n        \n        void calcResJacobianSingleFeatAllMonoObs(\n            const std::shared_ptr<FeatureInfo> feature_info,\n            const std::map<double, std::shared_ptr<SE3>>& sw_poses,\n            std::vector<std::shared_ptr<Type>>& block_var_order,\n            std::map<std::shared_ptr<Type>, int>& block_index_map,\n            Eigen::VectorXd& res_block,\n            Eigen::MatrixXd& H_block);\n        \n        void calcResJacobianSingleFeatAllStereoObs(\n            const std::shared_ptr<FeatureInfo> feature_info, \n            const std::map<double, std::shared_ptr<SE3>>& sw_poses,\n            const Eigen::Isometry3d& T_cl2cr,\n            std::vector<std::shared_ptr<Type>>& block_var_order,\n            std::map<std::shared_ptr<Type>, int>& block_index_map,\n            Eigen::VectorXd& res_block,\n            Eigen::MatrixXd& H_block);\n        \n        int _max_valid_ids;        \n        double _noise;\n    }; \n}\n"
  },
  {
    "path": "ingvio_estimator/src/State.cpp",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#include \"State.h\"\n\nnamespace ingvio\n{\n    StateParams::StateParams(const IngvioParams& filter_params)\n    {\n        _cam_nums = filter_params._cam_nums;\n        _max_sw_poses = filter_params._max_sw_clones;\n        _max_landmarks = filter_params._max_lm_feats;\n        \n        _T_cl2cr = filter_params._T_cr2i.inverse()*filter_params._T_cl2i;\n        _T_cl2i = filter_params._T_cl2i;\n        \n        _enable_gnss = static_cast<bool>(filter_params._enable_gnss);\n        \n        _noise_a = filter_params._noise_a;\n        _noise_g = filter_params._noise_g;\n        _noise_ba = filter_params._noise_ba;\n        _noise_bg = filter_params._noise_bg;\n        \n        _init_cov_rot = filter_params._init_cov_rot;\n        _init_cov_pos = filter_params._init_cov_pos;\n        _init_cov_vel = filter_params._init_cov_vel;\n        _init_cov_bg = filter_params._init_cov_bg;\n        _init_cov_ba = filter_params._init_cov_ba;\n        _init_cov_ext_rot = filter_params._init_cov_ext_rot;\n        _init_cov_ext_pos = filter_params._init_cov_ext_pos;\n        \n        if (_enable_gnss)\n        {\n            _noise_clockbias = filter_params._noise_clockbias;\n            _noise_clockbias = filter_params._noise_cb_rw;\n            \n            _init_cov_rcv_clockbias = filter_params._init_cov_rcv_clockbias;\n            _init_cov_rcv_clockbias_randomwalk = filter_params._init_cov_rcv_clockbias_randomwalk;\n            _init_cov_yof = filter_params._init_cov_yof;\n        }\n    }\n    \n    State::State(const IngvioParams& filter_params) : _state_params(filter_params)\n    {\n        int idx = 0;\n        \n        _extended_pose = std::make_shared<SE23>();\n        _extended_pose->set_cov_idx(idx);\n        _err_variables.push_back(_extended_pose);\n        idx += _extended_pose->size();\n        \n        _bg = std::make_shared<Vec3>();\n        _bg->set_cov_idx(idx);\n        _err_variables.push_back(_bg);\n        idx += _bg->size();\n        \n        _ba = std::make_shared<Vec3>();\n        _ba->set_cov_idx(idx);\n        _err_variables.push_back(_ba);\n        idx += _ba->size();\n        \n        _camleft_imu_extrinsics = std::make_shared<SE3>();\n        _camleft_imu_extrinsics->set_cov_idx(idx);\n        _err_variables.push_back(_camleft_imu_extrinsics);\n        idx += _camleft_imu_extrinsics->size();\n        \n        _gnss.clear();\n        _sw_camleft_poses.clear();\n        _anchored_landmarks.clear();\n        \n        _cov = std::pow(1e-03, 2)*Eigen::MatrixXd::Identity(idx, idx);\n        \n        _camleft_imu_extrinsics->setValueByIso(filter_params._T_cl2i);\n    }\n    \n    State::State() : _state_params()\n    {\n        int idx = 0;\n        \n        _extended_pose = std::make_shared<SE23>();\n        _extended_pose->set_cov_idx(idx);\n        _err_variables.push_back(_extended_pose);\n        idx += _extended_pose->size();\n        \n        _bg = std::make_shared<Vec3>();\n        _bg->set_cov_idx(idx);\n        _err_variables.push_back(_bg);\n        idx += _bg->size();\n        \n        _ba = std::make_shared<Vec3>();\n        _ba->set_cov_idx(idx);\n        _err_variables.push_back(_ba);\n        idx += _ba->size();\n        \n        _camleft_imu_extrinsics = std::make_shared<SE3>();\n        _camleft_imu_extrinsics->set_cov_idx(idx);\n        _err_variables.push_back(_camleft_imu_extrinsics);\n        idx += _camleft_imu_extrinsics->size();\n        \n        _gnss.clear();\n        _sw_camleft_poses.clear();\n        _anchored_landmarks.clear();\n        \n        _cov = std::pow(1e-03, 2)*Eigen::MatrixXd::Identity(idx, idx);\n        \n        _camleft_imu_extrinsics->setValueByIso(Eigen::Isometry3d::Identity());\n    }\n    \n    void State::initStateAndCov(double init_timestamp,\n                                const Eigen::Quaterniond& init_quat_i2w,\n                                const Eigen::Vector3d& init_pos,\n                                const Eigen::Vector3d& init_vel,\n                                const Eigen::Vector3d& init_bg,\n                                const Eigen::Vector3d& init_ba)\n    {\n        _timestamp = init_timestamp;\n        \n        for (int i = _extended_pose->idx(); i < _extended_pose->idx()+3; ++i)\n            _cov(i, i) = std::pow(_state_params._init_cov_rot, 2.0);\n        \n        for (int i = _extended_pose->idx()+3; i < _extended_pose->idx()+6; ++i)\n            _cov(i, i) = std::pow(_state_params._init_cov_pos, 2.0);\n        \n        for (int i = _extended_pose->idx()+6; i < _extended_pose->idx()+9; ++i)\n            _cov(i, i) = std::pow(_state_params._init_cov_vel, 2.0);\n        \n        for (int i = _bg->idx(); i < _bg->idx()+_bg->size(); ++i)\n            _cov(i, i) = std::pow(_state_params._init_cov_bg, 2.0);\n        \n        for (int i = _ba->idx(); i < _ba->idx()+_ba->size(); ++i)\n            _cov(i, i) = std::pow(_state_params._init_cov_ba, 2.0);\n        \n        for (int i = _camleft_imu_extrinsics->idx(); i < _camleft_imu_extrinsics->idx()+3; ++i)\n            _cov(i, i) = std::pow(_state_params._init_cov_ext_rot, 2.0);\n        \n        for (int i = _camleft_imu_extrinsics->idx()+3; i < _camleft_imu_extrinsics->idx()+6; ++i)\n            _cov(i, i) = std::pow(_state_params._init_cov_ext_pos, 2.0);\n        \n        this->_extended_pose->setValueLinearByQuat(init_quat_i2w);\n        \n        this->_extended_pose->setValueTrans1(init_pos);\n        \n        this->_extended_pose->setValueTrans2(init_vel);\n        \n        this->_bg->setValue(init_bg);\n        \n        this->_ba->setValue(init_ba);\n        \n        this->_camleft_imu_extrinsics->setValueByIso(_state_params._T_cl2i);\n    }\n    \n    void State::initStateAndCov(double init_timestamp,\n                                const Eigen::Quaterniond& init_quat_i2w,\n                                const Eigen::Vector3d& init_pos)\n    {\n        this->initStateAndCov(init_timestamp, init_quat_i2w, init_pos, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero());\n    }\n    \n    void State::initStateAndCov(double init_timestamp,\n                                const Eigen::Quaterniond& init_quat_i2w)\n    {\n        this->initStateAndCov(init_timestamp, init_quat_i2w, Eigen::Vector3d::Zero());\n    }\n    \n    double distance(std::shared_ptr<State> state1, std::shared_ptr<State> state2)\n    {\n        if (state1->_timestamp != state2->_timestamp)\n            std::cout << \"[State]: Timestamp not the same when calc distance!\" << std::endl; \n        \n        double dist = 0.0;\n        \n        dist += (state1->_extended_pose->valueLinearAsMat()-state2->_extended_pose->valueLinearAsMat()).norm();\n        \n        dist += (state1->_extended_pose->valueTrans1()-state2->_extended_pose->valueTrans1()).norm();\n        \n        dist += (state1->_extended_pose->valueTrans2()-state2->_extended_pose->valueTrans2()).norm();\n        \n        dist += (state1->_bg->value()-state2->_bg->value()).norm();\n        \n        dist += (state1->_ba->value()-state2->_ba->value()).norm();\n        \n        dist += (state1->_camleft_imu_extrinsics->valueLinearAsMat()-state2->_camleft_imu_extrinsics->valueLinearAsMat()).norm();\n        \n        dist += (state1->_camleft_imu_extrinsics->valueTrans()-state2->_camleft_imu_extrinsics->valueTrans()).norm();\n        \n        for (const auto& item : state1->_gnss)\n        {\n            assert(state2->_gnss.find(item.first) != state2->_gnss.end());\n            \n            dist += std::fabs(item.second->value() - state2->_gnss.at(item.first)->value());\n        }\n        \n        for (const auto& item : state1->_anchored_landmarks)\n        {\n            assert(state2->_anchored_landmarks.find(item.first) != state2->_anchored_landmarks.end());\n            \n            dist += (item.second->valuePosXyz()-state2->_anchored_landmarks.at(item.first)->valuePosXyz()).norm();\n        }\n        \n        for (const auto& item : state2->_sw_camleft_poses)\n        {\n            assert(state2->_sw_camleft_poses.find(item.first) != state2->_sw_camleft_poses.end());\n            \n            dist += (item.second->valueLinearAsMat()-state2->_sw_camleft_poses.at(item.first)->valueLinearAsMat()).norm();\n            \n            dist += (item.second->valueTrans()-state2->_sw_camleft_poses.at(item.first)->valueTrans()).norm();\n        }\n        \n        return  dist;\n    }\n\n}\n"
  },
  {
    "path": "ingvio_estimator/src/State.h",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#pragma once\n\n#include <map>\n#include <unordered_map>\n#include <memory>\n#include <vector>\n#include <functional>\n\n#include \"VecState.h\"\n#include \"PoseState.h\"\n#include \"AnchoredLandmark.h\"\n\n#include \"IngvioParams.h\"\n\nnamespace ingvio\n{\n    class StateParams\n    {\n    public:\n        StateParams() = default;\n        \n        StateParams(const IngvioParams& filter_params);\n        \n        int _cam_nums = 2;\n        int _max_sw_poses = 20;\n        int _max_landmarks = 25;\n        \n        double _noise_g = 0.005;\n        double _noise_a = 0.05;\n        double _noise_bg = 0.001;\n        double _noise_ba = 0.01;\n        double _noise_clockbias = 2.0;\n        double _noise_cb_rw = 0.2;\n        \n        double _init_cov_rot;\n        double _init_cov_pos;\n        double _init_cov_vel;\n        double _init_cov_bg;\n        double _init_cov_ba;\n        double _init_cov_ext_rot;\n        double _init_cov_ext_pos;\n        double _init_cov_rcv_clockbias;\n        double _init_cov_rcv_clockbias_randomwalk;\n        double _init_cov_yof;\n        \n        bool _enable_gnss = true;\n        \n        Eigen::Isometry3d _T_cl2cr = Eigen::Isometry3d::Identity();\n        Eigen::Isometry3d _T_cl2i = Eigen::Isometry3d::Identity();\n    };\n    \n    class State\n    {\n    public:\n        enum GNSSType {GPS = 0, GLO, GAL, BDS, FS, YOF};\n        \n        State();\n        State(const IngvioParams& filter_params);\n\n        \n        ~State() {}\n        \n        double nextMargTime()\n        {\n            double time = INFINITY;\n            \n            if (_sw_camleft_poses.size() > _state_params._max_sw_poses)\n                for (const auto& item: _sw_camleft_poses)\n                    if (item.first < time) time = item.first;\n                \n            return time;\n        }\n        \n        void initStateAndCov(double init_timestamp,\n                             const Eigen::Quaterniond& init_quat_i2w);\n        \n        void initStateAndCov(double init_timestamp,\n                             const Eigen::Quaterniond& init_quat_i2w,\n                             const Eigen::Vector3d& init_pos);\n        \n        void initStateAndCov(double init_timestamp,\n                             const Eigen::Quaterniond& init_quat_i2w,\n                             const Eigen::Vector3d& init_pos,\n                             const Eigen::Vector3d& init_vel,\n                             const Eigen::Vector3d& init_bg,\n                             const Eigen::Vector3d& init_ba);\n        \n        int curr_cov_size() { return _cov.rows(); }\n        int curr_err_variable_size() { return _err_variables.size();}\n        \n        double _timestamp = -1;\n        \n        StateParams _state_params;\n        \n        std::shared_ptr<SE23> _extended_pose;\n        \n        std::shared_ptr<Vec3> _bg;\n        \n        std::shared_ptr<Vec3> _ba;\n        \n        std::shared_ptr<SE3> _camleft_imu_extrinsics;\n        \n        std::unordered_map<GNSSType, std::shared_ptr<Scalar>> _gnss;\n        \n        std::unordered_map<int, std::shared_ptr<AnchoredLandmark>> _anchored_landmarks;\n        \n        std::map<double, std::shared_ptr<SE3>, std::less<double>> _sw_camleft_poses;\n        \n    private:\n        \n        friend class StateManager;\n        \n        Eigen::MatrixXd _cov;\n        \n        std::vector<std::shared_ptr<Type>> _err_variables;\n    };\n    \n    extern double distance(std::shared_ptr<State> state1, std::shared_ptr<State> state2);\n}\n\n\n"
  },
  {
    "path": "ingvio_estimator/src/StateManager.cpp",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#include <boost/math/distributions/chi_squared.hpp>\n\n#include \"StateManager.h\"\n\nnamespace ingvio\n{\n    bool StateManager::checkStateContinuity(const std::shared_ptr<State> state)\n    {\n        int idx = 0;\n        for (int i = 0; i < state->_err_variables.size(); ++i)\n            if (state->_err_variables[i]->idx() == idx)\n                idx += state->_err_variables[i]->size();\n            else\n                return false;\n            \n        if (state->_cov.rows() == idx && state->_cov.cols() == idx)\n            return true;\n        else\n            return false;\n    }  \n    \n    void StateManager::propagateStateCov(std::shared_ptr<State> state,\n                                         const Eigen::Matrix<double, 15, 15>& Phi_imu,\n                                         const Eigen::Matrix<double, 15, 12>& G_imu,\n                                         double dt)\n    {\n        assert(StateManager::checkStateContinuity(state));\n        \n        Eigen::MatrixXd cov_tmp(state->curr_cov_size(), state->curr_cov_size());\n        \n        cov_tmp.block<15, 15>(0, 0) = Phi_imu*state->_cov.block<15, 15>(0, 0)*Phi_imu.transpose();\n        \n        Eigen::MatrixXd cov21 = state->_cov.block(15, 0, state->_cov.rows()-15, 15)*Phi_imu.transpose();\n        Eigen::MatrixXd cov22 = state->_cov.block(15, 15, state->_cov.rows()-15, state->_cov.cols()-15);\n        \n        if (state->_state_params._enable_gnss && state->_gnss.find(State::GNSSType::FS) != state->_gnss.end())\n        {\n            Eigen::MatrixXd cov21_tmp = cov21;\n            Eigen::MatrixXd cov22_tmp = cov22;\n            \n            const int local_col_idx = state->_gnss.find(State::GNSSType::FS)->second->idx() - 15;\n            \n            for (int i = 0; i < 4; ++i)\n            {\n                auto item = state->_gnss.find(static_cast<State::GNSSType>(i));\n                if (item != state->_gnss.end())\n                {\n                    const int local_row_idx = item->second->idx() - 15;                    \n                    cov21_tmp.row(local_row_idx) += dt*cov21.row(local_col_idx);\n                    cov22_tmp.row(local_row_idx) += dt*cov22.row(local_col_idx);\n                }\n            }\n            cov21 = cov21_tmp;\n            \n            cov22 = cov22_tmp;\n            for (int i = 0; i < 4; ++i)\n            {\n                auto item = state->_gnss.find(static_cast<State::GNSSType>(i));\n                if (item != state->_gnss.end())\n                {\n                    const int local_row_idx = item->second->idx() - 15;                    \n                    cov22_tmp.col(local_row_idx) += dt*cov22.col(local_col_idx);\n                }\n            }\n            cov22 = cov22_tmp;\n        }\n        \n        cov_tmp.block(15, 0, cov_tmp.rows()-15, 15) = cov21;\n        cov_tmp.block(0, 15, 15, cov_tmp.cols()-15) = cov21.transpose();\n        cov_tmp.block(15, 15, cov_tmp.rows()-15, cov_tmp.cols()-15) = cov22;\n        \n        Eigen::Matrix<double, 15, 12> G_tmp = G_imu;\n        G_tmp.middleCols<3>(0) *= state->_state_params._noise_g;\n        G_tmp.middleCols<3>(3) *= state->_state_params._noise_a;\n        G_tmp.middleCols<3>(6) *= state->_state_params._noise_bg;\n        G_tmp.middleCols<3>(9) *= state->_state_params._noise_ba;\n        cov_tmp.block<15, 15>(0, 0) += dt*Phi_imu*G_tmp*G_tmp.transpose()*Phi_imu.transpose();\n        \n        if (state->_state_params._enable_gnss)\n            for (int i = 0; i < 5; ++i)\n            {\n                auto item1 = state->_gnss.find(static_cast<State::GNSSType>(i));\n                if (item1 == state->_gnss.end()) continue;\n                for (int j = 0; j < 5; ++j)\n                {\n                    auto item2 = state->_gnss.find(static_cast<State::GNSSType>(j));\n                    if (item2 == state->_gnss.end()) continue;\n                    \n                    if (item1->first != State::GNSSType::FS && item2->first != State::GNSSType::FS)\n                        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);\n                    else if (item1->first == State::GNSSType::FS && item2->first == State::GNSSType::FS)\n                        cov_tmp(item1->second->idx(), item2->second->idx()) += dt*std::pow(state->_state_params._noise_cb_rw, 2); \n                    else\n                        cov_tmp(item1->second->idx(), item2->second->idx()) += std::pow(dt, 2)*std::pow(state->_state_params._noise_cb_rw, 2);\n                }\n            }\n            \n        state->_cov = 0.5*(cov_tmp + cov_tmp.transpose());\n    }\n    \n    Eigen::MatrixXd StateManager::getFullCov(std::shared_ptr<State> state)\n    {\n        Eigen::MatrixXd cov(state->curr_cov_size(), state->curr_cov_size());\n        cov = state->_cov;\n        return cov;\n    }\n    \n    Eigen::MatrixXd StateManager::getMarginalCov(std::shared_ptr<State> state,\n                                                 const std::vector<std::shared_ptr<Type>>& small_variables)\n    {\n        int small_cov_size = 0;\n        \n        for (int i = 0; i < small_variables.size(); ++i)\n            small_cov_size += small_variables[i]->size();\n        \n        Eigen::MatrixXd small_cov(small_cov_size, small_cov_size);\n        \n        int row_idx = 0;\n        for (int i = 0; i < small_variables.size(); ++i)\n        {\n            int col_idx = 0;\n            for(int j = 0; j < small_variables.size(); ++j)\n            {\n                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());\n                \n                col_idx += small_variables[j]->size();\n            }\n            \n            row_idx += small_variables[i]->size();\n        }\n        \n        return small_cov;\n    }\n    \n    void StateManager::marginalize(std::shared_ptr<State> state, std::shared_ptr<Type> marg)\n    {\n        if (std::find(state->_err_variables.begin(), state->_err_variables.end(), marg) == state->_err_variables.end() )\n        {\n            std::cout << \"[StateManager]: Marg is not in the current state!\" << std::endl;\n            std::exit(EXIT_FAILURE);\n        }\n        \n        int marg_size = marg->size();\n        int marg_start = marg->idx();\n        int marg_end = marg->idx() + marg->size();\n        \n        Eigen::MatrixXd cov_tmp(state->_cov.rows() - marg_size, state->_cov.cols() - marg_size);\n        \n        cov_tmp.block(0, 0, marg_start, marg_start) = state->_cov.block(0, 0, marg_start, marg_start);\n        \n        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);\n        \n        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);\n        \n        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);\n        \n        state->_cov = cov_tmp;\n        \n        std::vector<std::shared_ptr<Type>> remaining_variables;\n        for (int i = 0; i < state->_err_variables.size(); ++i)\n            if (state->_err_variables[i] != marg)\n            {\n                if (state->_err_variables[i]->idx() > marg_start)\n                    state->_err_variables[i]->set_cov_idx(state->_err_variables[i]->idx()-marg_size);\n                    \n                remaining_variables.push_back(state->_err_variables[i]);\n            }\n            \n        marg->set_cov_idx(-1);\n        \n        state->_err_variables = remaining_variables;\n    }\n    \n    void StateManager::addVariableIndependent(std::shared_ptr<State> state,\n                                              std::shared_ptr<Type> new_state,\n                                              const Eigen::MatrixXd& new_state_cov_block)\n    {\n        assert(new_state->size() == new_state_cov_block.rows());\n        \n        int old_cov_size = state->curr_cov_size();\n        \n        Eigen::MatrixXd new_cov(old_cov_size+new_state->size(), old_cov_size+new_state->size());\n        new_cov.setZero();\n        \n        new_cov.block(0, 0, old_cov_size, old_cov_size) = state->_cov;\n        new_cov.block(old_cov_size, old_cov_size, new_state->size(), new_state->size()) = new_state_cov_block;\n        \n        new_state->set_cov_idx(old_cov_size);\n        \n        state->_cov = new_cov;\n        state->_err_variables.push_back(new_state);\n        \n        assert(StateManager::checkStateContinuity(state));\n    }\n    \n    void StateManager::addGNSSVariable(std::shared_ptr<State> state,\n                                       const State::GNSSType& gtype,\n                                       double value,\n                                       double cov)\n    {\n        if (state->_gnss.find(gtype) != state->_gnss.end())\n            std::cout << \"[StateManager]: GNSS variable already in the state, adding operation will rewrite such var!\" << std::endl;\n        \n        state->_gnss[gtype] = std::make_shared<Scalar>();\n        state->_gnss[gtype]->setValue(value);\n        \n        Eigen::MatrixXd scalar_cov(1, 1);\n        scalar_cov(0, 0) = cov;\n        \n        StateManager::addVariableIndependent(state, state->_gnss[gtype], scalar_cov);\n    }\n    \n    void StateManager::margGNSSVariable(std::shared_ptr<State> state,\n                                        const State::GNSSType& gtype)\n    {\n        if (state->_gnss.find(gtype) == state->_gnss.end())\n            std::cout << \"[StateManager]: GNSS variable not in the state, no need to marg!\" << std::endl;\n        \n        StateManager::marginalize(state, state->_gnss.at(gtype));\n        \n        state->_gnss.erase(gtype);\n    }\n    \n    void StateManager::boxPlus(std::shared_ptr<State> state, const Eigen::VectorXd& dx)\n    {\n        assert(dx.rows() == state->curr_cov_size());\n        assert(StateManager::checkStateContinuity(state));\n        \n        for (int i = 0; i < state->_err_variables.size(); ++i)\n            state->_err_variables[i]->update(dx);\n    }\n    \n    void StateManager::augmentSlidingWindowPose(std::shared_ptr<State> state)\n    {\n        if (state->_sw_camleft_poses.find(state->_timestamp) != state->_sw_camleft_poses.end())\n        {\n            std::cout << \"[StateManager]: Curr pose already in the sw, cannot clone!\" << std::endl;\n            return;\n        }\n        \n        std::shared_ptr<SE3> leftcam_pose_clone = std::shared_ptr<SE3>(new SE3());\n        \n        Eigen::Isometry3d T_i2w = Eigen::Isometry3d::Identity();\n        T_i2w.linear() = state->_extended_pose->valueLinearAsMat();\n        T_i2w.translation() = state->_extended_pose->valueTrans1();\n        \n        Eigen::Isometry3d T_cl2i = Eigen::Isometry3d::Identity();\n        T_cl2i.linear() = state->_camleft_imu_extrinsics->valueLinearAsMat();\n        T_cl2i.translation() = state->_camleft_imu_extrinsics->valueTrans();\n        \n        leftcam_pose_clone->setValueByIso(T_i2w*T_cl2i);\n        leftcam_pose_clone->setFejByIso(T_i2w*T_cl2i);\n        \n        leftcam_pose_clone->set_cov_idx(state->curr_cov_size());\n        \n        state->_sw_camleft_poses[state->_timestamp] = leftcam_pose_clone;\n        state->_err_variables.push_back(leftcam_pose_clone);\n        \n        Eigen::Matrix<double, 6, 21> J = Eigen::Matrix<double, 6, 21>::Zero();\n        J.block<6, 6>(0, 0).setIdentity();\n        J.block<3, 3>(0, 15) = state->_extended_pose->valueLinearAsMat();\n        J.block<3, 3>(3, 18) = state->_extended_pose->valueLinearAsMat();\n        \n        Eigen::MatrixXd cov_new(state->_cov.rows()+leftcam_pose_clone->size(), state->_cov.cols()+leftcam_pose_clone->size());\n        cov_new.setZero();\n        \n        cov_new.block(0, 0, state->curr_cov_size(), state->curr_cov_size()) = state->_cov;\n        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();\n        \n        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());\n        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();\n        \n        state->_cov = 0.5*(cov_new+cov_new.transpose());\n        \n        assert(checkStateContinuity(state));\n    }\n    \n    void StateManager::addAnchoredLandmarkInState(std::shared_ptr<State> state,\n                                                  std::shared_ptr<AnchoredLandmark> anchored_landmark,\n                                                  int lm_id,\n                                                  const Eigen::Matrix3d& cov)\n    {\n        if (state->_anchored_landmarks.find(lm_id) != state->_anchored_landmarks.end())\n        {\n            std::cout << \"[StateManager]: Landmark already in the state, cannot add!\" << std::endl;\n            return;\n        }\n        \n        state->_anchored_landmarks[lm_id] = anchored_landmark;\n        \n        StateManager::addVariableIndependent(state, anchored_landmark, cov);\n        \n        assert(StateManager::checkStateContinuity(state));\n    }\n    \n    void StateManager::margSlidingWindowPose(std::shared_ptr<State> state, double marg_time)\n    {\n        if (state->_sw_camleft_poses.find(marg_time) == state->_sw_camleft_poses.end())\n            std::cout << \"[StateManager]: Marg pose time not exists! Cannot marg!\" << std::endl;\n        \n        StateManager::marginalize(state, state->_sw_camleft_poses.at(marg_time));\n        \n        state->_sw_camleft_poses.erase(marg_time);\n        \n        assert(StateManager::checkStateContinuity(state));\n    }\n    \n    void StateManager::margSlidingWindowPose(std::shared_ptr<State> state)\n    {\n        double marg_time = state->nextMargTime();\n        if (marg_time == INFINITY)\n        {\n            std::cout << \"[StateManager]: Auto marg pose gives inf time! Cannot marg!\" << std::endl;\n            return;\n        }\n        \n        StateManager::margSlidingWindowPose(state, marg_time);\n    }\n    \n    void StateManager::margAnchoredLandmarkInState(std::shared_ptr<State> state, int lm_id)\n    {\n        if (state->_anchored_landmarks.find(lm_id) == state->_anchored_landmarks.end())\n        {\n            std::cout << \"[StateManager]: Landmark id not exists in state! Cannot marg!\" << std::endl;\n            return;\n        }\n        \n        StateManager::marginalize(state, state->_anchored_landmarks.at(lm_id));\n        \n        state->_anchored_landmarks.erase(lm_id);\n        \n        assert(StateManager::checkStateContinuity(state));\n    }\n    \n    // Acknowledgements: This function is inspired by the realization of OpenVINS\n    // Delayed-Initialization\n    // See https://github.com/rpng/open_vins\n    \n    void StateManager::ekfUpdate(std::shared_ptr<State> state,\n                                 const std::vector<std::shared_ptr<Type>>& var_order,\n                                 const Eigen::MatrixXd& H,\n                                 const Eigen::VectorXd& res,\n                                 const Eigen::MatrixXd& R)\n    {\n        assert(res.rows() == R.rows());\n        assert(H.rows() == res.rows());\n        assert(R.rows() == R.cols());\n        \n        assert(StateManager::checkSubOrder(state, var_order));\n        \n        int small_idx = 0;\n        std::vector<int> H_idx;\n        \n        for (int i = 0; i < var_order.size(); ++i)\n        {\n            H_idx.push_back(small_idx);\n            \n            small_idx += var_order[i]->size();\n        }\n        \n        Eigen::MatrixXd PH_T(state->_cov.rows(), H.rows());\n       \n        PH_T.setZero();\n        \n        for (const auto& total_var : state->_err_variables)\n        {\n            Eigen::MatrixXd PH_T_i = Eigen::MatrixXd::Zero(total_var->size(), res.rows());\n            \n            for (int i = 0; i < var_order.size(); ++i)\n            {\n                std::shared_ptr<Type> meas_var = var_order[i];\n                \n                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();\n            }\n            \n            PH_T.block(total_var->idx(), 0, total_var->size(), res.rows()) = PH_T_i;\n        }\n        \n        Eigen::MatrixXd small_cov = StateManager::getMarginalCov(state, var_order);\n        \n        Eigen::MatrixXd S(R.rows(), R.cols());\n        \n        S = H*small_cov*H.transpose() + R;\n        \n        Eigen::MatrixXd K = PH_T*S.inverse();\n        \n        Eigen::MatrixXd cov_tmp = state->_cov;\n        \n        cov_tmp -= K*PH_T.transpose();\n        \n        state->_cov = 0.5*(cov_tmp + cov_tmp.transpose());\n        \n        Eigen::VectorXd diags = state->_cov.diagonal();\n        \n        for (int i = 0; i < diags.rows(); ++i)\n            if (diags(i) < 0.0)\n            {\n                std::cout << \"[StateManager]: EKF Update and found negative diag cov elements! \" << std::endl;\n                \n                assert(false);\n            }\n            \n        Eigen::VectorXd dx = K*res;\n        \n        StateManager::boxPlus(state, dx);\n    }\n    \n    bool StateManager::checkSubOrder(std::shared_ptr<State> state,\n                                     const std::vector<std::shared_ptr<Type>>& sub_order)\n    {\n        auto isFound = [&state] (const std::shared_ptr<Type>& var)\n        {\n            return std::find(state->_err_variables.begin(), state->_err_variables.end(), var) !=  state->_err_variables.end();\n        };\n        \n        for (const auto& item : sub_order)\n            if (!isFound(item))\n            {\n                std::cout << \"[StateManager]: Existing sub order var not in state! \" << std::endl;\n                \n                return false;\n            }\n                \n        return true;\n    }\n    \n    int StateManager::calcSubVarSize(const std::vector<std::shared_ptr<Type>>& sub_var)\n    {\n        int total_size = 0;\n        \n        for (const auto& item : sub_var)\n            if (item != nullptr)\n                total_size += item->size();\n        \n        return total_size;\n    }\n    \n    // Acknowledgements: This function is inspired by the realization of OpenVINS\n    // Delayed-Initialization\n    // See https://github.com/rpng/open_vins\n    \n    void StateManager::addVariableDelayedInvertible(std::shared_ptr<State> state,\n                                                    std::shared_ptr<Type> var_new,\n                                                    const std::vector<std::shared_ptr<Type>>& var_old_order,\n                                                    const Eigen::MatrixXd& H_old,\n                                                    const Eigen::MatrixXd& H_new,\n                                                    const Eigen::VectorXd& res,\n                                                    double noise_iso_meas)\n    {\n        if (std::find(state->_err_variables.begin(), state->_err_variables.end(), var_new) != state->_err_variables.end())\n        {\n            std::cout << \"[StateManager]: New var already in state! Cannot perform add var delayed inv!\" << std::endl;\n            return;\n        }\n        \n        assert(StateManager::checkSubOrder(state, var_old_order));\n        \n        int old_sub_var_size = StateManager::calcSubVarSize(var_old_order);\n        \n        assert(res.rows() == H_old.rows());\n        assert(res.rows() == H_new.rows());\n        assert(H_new.rows() == H_new.cols());\n        assert(H_new.cols() == var_new->size());\n        assert(H_old.cols() == old_sub_var_size);\n        \n        assert(H_new.determinant() != 0);\n        \n        int small_idx = 0;\n        std::vector<int> H_idx;\n        \n        for (int i = 0; i < var_old_order.size(); ++i)\n        {\n            H_idx.push_back(small_idx);\n            small_idx += var_old_order[i]->size();\n        }\n        \n        Eigen::MatrixXd PH_T = Eigen::MatrixXd::Zero(state->_cov.rows(), res.rows());\n        \n        for (int i = 0; i < state->_err_variables.size(); ++i)\n        {\n            Eigen::MatrixXd PH_T_i = Eigen::MatrixXd::Zero(state->_err_variables[i]->size(), res.rows());\n            \n            for (int j = 0; j < var_old_order.size(); ++j)\n            {\n                std::shared_ptr<Type> meas_var = var_old_order[j];\n                \n                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();\n            }\n            \n            PH_T.block(state->_err_variables[i]->idx(), 0, state->_err_variables[i]->size(), res.rows()) = PH_T_i;\n        }\n        \n        Eigen::MatrixXd small_cov = StateManager::getMarginalCov(state, var_old_order);\n        \n        Eigen::MatrixXd S(res.rows(), res.rows());\n        \n        S = H_old*small_cov*H_old.transpose();\n        \n        for (int i = 0; i < S.cols(); ++i)\n            S(i, i) += std::pow(noise_iso_meas, 2.0);\n        \n        Eigen::MatrixXd H_new_inv = H_new.inverse();\n        \n        Eigen::MatrixXd cov_newnew = H_new_inv*S*H_new_inv.transpose();\n        \n        Eigen::MatrixXd cov_tmp = Eigen::MatrixXd::Zero(state->_cov.rows() + var_new->size(), state->_cov.cols() + var_new->size());\n        \n        cov_tmp.block(0, 0, state->_cov.rows(), state->_cov.cols()) = state->_cov;\n        \n        cov_tmp.block(state->_cov.rows(), state->_cov.cols(), var_new->size(), var_new->size()) = cov_newnew;\n        \n        cov_tmp.block(0, state->_cov.cols(), state->_cov.rows(), var_new->size()) = -PH_T*H_new_inv.transpose();\n        \n        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();\n        \n        var_new->set_cov_idx(state->_cov.cols());\n        \n        state->_err_variables.push_back(var_new);\n        \n        state->_cov = 0.5*(cov_tmp + cov_tmp.transpose());\n    }\n    \n    // Acknowledgements: This function is inspired by the realization of OpenVINS\n    // Delayed-Initialization\n    // See https://github.com/rpng/open_vins\n    \n    bool StateManager::addVariableDelayed(std::shared_ptr<State> state,\n                                          std::shared_ptr<Type> var_new,\n                                          const std::vector<std::shared_ptr<Type>>& var_old_order,\n                                          Eigen::MatrixXd& H_old,\n                                          Eigen::MatrixXd& H_new,\n                                          Eigen::VectorXd& res,\n                                          double noise_iso_meas,\n                                          double chi2_mult_factor,\n                                          bool do_chi2)\n    {\n        if (std::find(state->_err_variables.begin(), state->_err_variables.end(), var_new) != state->_err_variables.end())\n        {\n            std::cout << \"[StateManager]: New var already in state! Cannot perform add var delayed inv!\" << std::endl;\n            return false;\n        }\n        \n        int old_sub_var_size = StateManager::calcSubVarSize(var_old_order);\n        int new_var_size = var_new->size();\n        \n        assert(StateManager::checkSubOrder(state, var_old_order));\n        \n        assert(res.rows() == H_old.rows());\n        assert(res.rows() == H_new.rows());\n\n        assert(H_new.cols() == new_var_size);\n        assert(H_old.cols() == old_sub_var_size);\n        \n        if (H_new.rows() <= H_new.cols())\n        {\n            std::cout << \"[StateManager]: H_new rows should be larger than H_new cols!\" << std::endl;\n            return false;\n        }\n        \n        Eigen::JacobiRotation<double> tmpG;\n        \n        for (int n = 0; n < H_new.cols(); ++n)\n            for (int m = H_new.rows()-1; m > n; --m)\n            {\n                tmpG.makeGivens(H_new(m-1, n), H_new(m, n));\n                \n                (H_new.block(m-1, n, 2, H_new.cols()-n)).applyOnTheLeft(0, 1,tmpG.adjoint());\n                \n                (res.block(m-1, 0, 2, 1)).applyOnTheLeft(0, 1, tmpG.adjoint());\n                \n                (H_old.block(m-1, 0, 2, H_old.cols())).applyOnTheLeft(0, 1, tmpG.adjoint());\n            }\n        \n        Eigen::MatrixXd Hxinit = H_old.block(0, 0, new_var_size, H_old.cols());\n        \n        Eigen::MatrixXd Hfinit = H_new.block(0, 0, new_var_size, new_var_size);\n        \n        Eigen::VectorXd resinit = res.block(0, 0, new_var_size, 1);\n        \n        Eigen::MatrixXd Hup = H_old.block(new_var_size, 0, H_old.rows()-new_var_size, H_old.cols());\n        \n        Eigen::VectorXd resup = res.block(new_var_size, 0, res.rows()-new_var_size, 1);\n        \n        Eigen::MatrixXd small_cov = StateManager::getMarginalCov(state, var_old_order);\n        \n        Eigen::MatrixXd S = Hup*small_cov*Hup.transpose();\n        \n        for (int i = 0; i < S.rows(); ++i)\n            S(i, i) += std::pow(noise_iso_meas, 2.0);\n        \n        double chi2 = resup.dot(S.llt().solve(resup));\n        \n        boost::math::chi_squared chi_squared_dist(res.rows());\n        \n        double chi2_check = boost::math::quantile(chi_squared_dist, 0.95);\n        \n        if (chi2 > chi2_mult_factor * chi2_check && do_chi2)\n        {\n            std::cout << \"[StateManager]: Cannot add variable due to chi2 test failure!\" << std::endl;\n            return false;\n        }\n        \n        StateManager::addVariableDelayedInvertible(state, var_new, var_old_order, Hxinit, Hfinit, resinit, noise_iso_meas);\n        \n        \n        if (Hup.rows() > 0)\n            StateManager::ekfUpdate(state, var_old_order, Hup, resup, std::pow(noise_iso_meas, 2.0)*Eigen::MatrixXd::Identity(resup.rows(), resup.rows()));\n        \n        return true;\n    }\n    \n    void StateManager::replaceVarLinear(std::shared_ptr<State> state,\n                                        const std::shared_ptr<Type> target_var,\n                                        const std::vector<std::shared_ptr<Type>>& dependence_order,\n                                        const Eigen::MatrixXd& H)\n    {\n        bool notFound = true;\n        for (const auto& item : state->_err_variables)\n            if (target_var == item)\n            {\n                notFound = false;\n                break;\n            }\n            \n        if (notFound)\n        {\n            std::cout << \"[StateManager]: Target var not in state, cannot linearly replace!\" << std::endl;\n            return;\n        }\n        \n        assert(StateManager::checkSubOrder(state, dependence_order));\n        assert(target_var->size() == H.rows());\n        \n        int small_idx = 0;\n        std::vector<int> H_idx;\n        \n        for (int i = 0; i < dependence_order.size(); ++i)\n        {\n            H_idx.push_back(small_idx);\n            \n            small_idx += dependence_order[i]->size();\n        }\n        \n        assert(small_idx == H.cols());\n        \n        Eigen::MatrixXd PH_T(state->_cov.rows(), H.rows());\n        \n        PH_T.setZero();\n        \n        for (const auto& total_var : state->_err_variables)\n        {\n            Eigen::MatrixXd PH_T_i = Eigen::MatrixXd::Zero(total_var->size(), H.rows());\n            \n            for (int i = 0; i < dependence_order.size(); ++i)\n            {\n                std::shared_ptr<Type> meas_var = dependence_order[i];\n                \n                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();\n            }\n            \n            PH_T.block(total_var->idx(), 0, total_var->size(), H.rows()) = PH_T_i;\n        }\n        \n        Eigen::MatrixXd small_cov = StateManager::getMarginalCov(state, dependence_order);\n        \n        Eigen::MatrixXd HPH_T = H*small_cov*H.transpose();\n        \n        state->_cov.block(0, target_var->idx(), state->curr_cov_size(), target_var->size()) = PH_T;\n        \n        state->_cov.block(target_var->idx(), 0, target_var->size(), state->curr_cov_size()) = PH_T.transpose();\n        \n        state->_cov.block(target_var->idx(), target_var->idx(), target_var->size(), target_var->size()) = HPH_T;\n    }\n}\n"
  },
  {
    "path": "ingvio_estimator/src/StateManager.h",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#pragma once\n\n#include <Eigen/Core>\n#include <Eigen/Geometry>\n#include <memory>\n\n#include \"State.h\"\n\nnamespace ingvio\n{\n    class State;\n    \n    class StateManager\n    {\n    public:\n        StateManager() = delete;\n        \n        static bool checkStateContinuity(const std::shared_ptr<State> state);\n        \n        static void propagateStateCov(std::shared_ptr<State> state,\n                                      const Eigen::Matrix<double, 15, 15>& Phi_imu,\n                                      const Eigen::Matrix<double, 15, 12>& G_imu,\n                                      double dt);\n        \n        static Eigen::MatrixXd getFullCov(std::shared_ptr<State> state);\n        \n        static Eigen::MatrixXd getMarginalCov(\n            std::shared_ptr<State> state,\n            const std::vector<std::shared_ptr<Type>>& small_variables);\n        \n        static void marginalize(std::shared_ptr<State> state, std::shared_ptr<Type> marg);\n        \n        static void addVariableIndependent(std::shared_ptr<State> state,\n                                           std::shared_ptr<Type> new_state,\n                                           const Eigen::MatrixXd& new_state_cov_block);\n        \n        static void addGNSSVariable(std::shared_ptr<State> state,\n                                    const State::GNSSType& gtype,\n                                    double value,\n                                    double cov);\n        \n        static void margGNSSVariable(std::shared_ptr<State> state, const State::GNSSType& gtype);\n        \n        static void boxPlus(std::shared_ptr<State> state, const Eigen::VectorXd& dx);\n        \n        static void augmentSlidingWindowPose(std::shared_ptr<State> state);\n        \n        static void addAnchoredLandmarkInState(\n            std::shared_ptr<State> state,\n            std::shared_ptr<AnchoredLandmark> anchored_landmark,\n            int lm_id,\n            const Eigen::Matrix3d& cov);\n        \n        static void margSlidingWindowPose(std::shared_ptr<State> state, double marg_time);\n        \n        static void margSlidingWindowPose(std::shared_ptr<State> state);\n        \n        static void margAnchoredLandmarkInState(std::shared_ptr<State> state, int lm_id);\n        \n        // Acknowledgements: This function is inspired by the realization of OpenVINS\n        // Delayed-Initialization\n        // See https://github.com/rpng/open_vins\n        \n        static void ekfUpdate(std::shared_ptr<State> state,\n                              const std::vector<std::shared_ptr<Type>>& var_order,\n                              const Eigen::MatrixXd& H,\n                              const Eigen::VectorXd& res,\n                              const Eigen::MatrixXd& R);\n        \n        static bool checkSubOrder(std::shared_ptr<State> state,\n                                  const std::vector<std::shared_ptr<Type>>& sub_order);\n        \n        static int calcSubVarSize(const std::vector<std::shared_ptr<Type>>& sub_var);\n        \n        // Acknowledgements: This function is inspired by the realization of OpenVINS\n        // Delayed-Initialization\n        // See https://github.com/rpng/open_vins\n        \n        static void addVariableDelayedInvertible(\n            std::shared_ptr<State> state,\n            std::shared_ptr<Type> var_new,\n            const std::vector<std::shared_ptr<Type>>& var_old_order,\n            const Eigen::MatrixXd& H_old,\n            const Eigen::MatrixXd& H_new,\n            const Eigen::VectorXd& res,\n            double noise_iso_meas);\n        \n        // Acknowledgements: This function is inspired by the realization of OpenVINS\n        // Delayed-Initialization\n        // See https://github.com/rpng/open_vins\n        \n        static bool addVariableDelayed(\n            std::shared_ptr<State> state,\n            std::shared_ptr<Type> var_new,\n            const std::vector<std::shared_ptr<Type>>& var_old_order,\n            Eigen::MatrixXd& H_old,\n            Eigen::MatrixXd& H_new,\n            Eigen::VectorXd& res,\n            double noise_iso_meas,\n            double chi2_mult_factor,\n            bool do_chi2 = true);\n        \n        static void replaceVarLinear(\n            std::shared_ptr<State> state,\n            const std::shared_ptr<Type> target_var,\n            const std::vector<std::shared_ptr<Type>>& dependence_order,\n            const Eigen::MatrixXd& H);\n    };\n}\n"
  },
  {
    "path": "ingvio_estimator/src/SwMargUpdate.cpp",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#include <Eigen/SVD>\n\n#include <Eigen/SparseCore>\n#include <Eigen/QR>\n#include <Eigen/SPQRSupport>\n\n#include \"IngvioParams.h\"\n\n#include \"StateManager.h\"\n\n#include \"MapServer.h\"\n#include \"MapServerManager.h\"\n\n#include \"Triangulator.h\"\n\n#include \"SwMargUpdate.h\"\n\n#include \"TicToc.h\"\n\nnamespace ingvio\n{\n    void SwMargUpdate::updateStateMono(std::shared_ptr<State> state,\n                                       std::shared_ptr<MapServer> map_server,\n                                       std::shared_ptr<Triangulator> tri)\n    {\n        double marg_time = state->nextMargTime();\n        \n        if (marg_time == INFINITY) return;\n        \n        std::vector<double> selected_timestamps;\n        this->selectSwTimestamps(state->_sw_camleft_poses, marg_time, selected_timestamps);\n        \n        std::vector<std::shared_ptr<SE3>> sw_var_order;\n        std::map<std::shared_ptr<SE3>, int> sw_index_map;\n        std::vector<std::shared_ptr<Type>> sw_var_type;\n        \n        this->generateSwVarOrder(state, selected_timestamps, sw_var_order,\n                                 sw_index_map, sw_var_type);\n        \n        std::vector<int> update_ids;\n        \n        for (const auto& item : *map_server)\n        {\n            const auto& feat_info_ptr = item.second;\n            \n            if (feat_info_ptr->_ftype != FeatureInfo::FeatureType::MSCKF)\n                continue;\n            \n            bool flag = false;\n            const auto& mono_obs = item.second->_mono_obs;\n            \n            for (const double& ts : selected_timestamps)\n                if (mono_obs.find(ts) == mono_obs.end())\n                {\n                    flag = true;\n                    break;\n                }\n                \n            if (flag) continue;\n            \n            if (FeatureInfoManager::triangulateFeatureInfoMono(feat_info_ptr, tri, state))\n                update_ids.push_back(item.first);\n        }\n        \n        if (update_ids.size() == 0) return;\n        \n        const int max_possible_cols = 6*state->_sw_camleft_poses.size();\n        \n        const int max_possible_rows = update_ids.size()*(2*selected_timestamps.size()-3);\n        \n        Eigen::VectorXd res_large(max_possible_rows);\n        Eigen::MatrixXd H_large(max_possible_rows, max_possible_cols);\n        res_large.setZero();\n        H_large.setZero();\n        \n        int row_cnt = 0;\n        int col_cnt = 6*sw_var_type.size();\n        \n        int feats_cnt = 0;\n        \n        for (int i = 0; i < update_ids.size(); ++i)\n        {\n            Eigen::VectorXd res_block;\n            Eigen::MatrixXd H_block;\n            Eigen::MatrixXd H_anchor_block;\n            \n            const auto& anchor_pose_ptr = this->calcResJacobianSingleFeatSelectedMonoObs(\n                map_server->at(update_ids[i]), state->_sw_camleft_poses,\n                sw_var_order, sw_index_map, selected_timestamps,\n                res_block, H_block, H_anchor_block);\n            \n            bool flag = false;\n            \n            if (sw_index_map.find(anchor_pose_ptr) == sw_index_map.end())\n            {\n                flag = true;\n                \n                sw_var_order.push_back(anchor_pose_ptr);\n                sw_var_type.push_back(anchor_pose_ptr);\n                sw_index_map[anchor_pose_ptr] = col_cnt;\n                \n                col_cnt += anchor_pose_ptr->size();\n                \n                H_block.conservativeResize(H_block.rows(), H_block.cols() + 6);\n            }\n            \n            H_block.block(0, sw_index_map.at(anchor_pose_ptr), H_block.rows(), 6) = H_anchor_block;\n            \n            if (!testChiSquared(state, res_block, H_block, sw_var_type, \n                this->_noise, selected_timestamps.size()-1))\n            {\n                if (flag)\n                {\n                    sw_var_order.pop_back();\n                    sw_var_type.pop_back();\n                    sw_index_map.erase(anchor_pose_ptr);\n                    \n                    col_cnt -= anchor_pose_ptr->size();\n                }\n                \n                continue;\n            }\n            \n            H_large.block(row_cnt, 0, H_block.rows(), col_cnt) = H_block;\n            res_large.block(row_cnt, 0, res_block.rows(), 1) = res_block;\n            \n            row_cnt += res_block.rows();\n            \n            ++feats_cnt;\n        }\n        \n        if (row_cnt < max_possible_rows || col_cnt < max_possible_cols)\n        {\n            H_large.conservativeResize(row_cnt, col_cnt);\n            res_large.conservativeResize(row_cnt, 1);\n        }\n        \n        Eigen::MatrixXd H_thin;\n        Eigen::VectorXd res_thin;\n        \n        if (H_large.rows() > H_large.cols())\n        {\n            Eigen::SparseMatrix<double> H_sparse = H_large.sparseView();\n            \n            Eigen::SPQR<Eigen::SparseMatrix<double>> spqr_helper;\n            spqr_helper.setSPQROrdering(SPQR_ORDERING_NATURAL);\n            spqr_helper.compute(H_sparse);\n            \n            Eigen::MatrixXd H_temp;\n            Eigen::VectorXd r_temp;\n            \n            (spqr_helper.matrixQ().transpose()*H_large).evalTo(H_temp);\n            (spqr_helper.matrixQ().transpose()*res_large).evalTo(r_temp);\n            \n            H_thin = H_temp.topRows(col_cnt);\n            res_thin = r_temp.head(col_cnt);                 \n        }\n        else\n        {\n            H_thin = H_large;\n            res_thin = res_large;\n        }\n        \n        /*\n        std::cout << \"[SwMargUpdate]: Num of mono feats used in selected pose = \" << feats_cnt << std::endl;\n        */\n        \n        StateManager::ekfUpdate(state, sw_var_type, H_thin, res_thin, std::pow(this->_noise, 2)*Eigen::MatrixXd::Identity(res_thin.rows(), res_thin.rows()));\n    }\n    \n    void SwMargUpdate::cleanMonoObsAtMargTime(std::shared_ptr<State> state,\n                                              std::shared_ptr<MapServer> map_server)\n    {\n        double marg_time = state->nextMargTime();\n        \n        if (marg_time == INFINITY) return;\n        \n        std::vector<int> all_ids;\n        for (const auto& item : *map_server)\n            all_ids.push_back(item.first);\n        \n        std::vector<int> ids_to_clean;\n        for (int i = 0; i < all_ids.size(); ++i)\n        {\n            if (map_server->at(all_ids[i])->_mono_obs.find(marg_time) != map_server->at(all_ids[i])->_mono_obs.end())\n                map_server->at(all_ids[i])->_mono_obs.erase(marg_time);\n            \n            if (map_server->at(all_ids[i])->_mono_obs.size() == 0)\n                ids_to_clean.push_back(all_ids[i]);\n        }\n        \n        for (int id : ids_to_clean)\n            map_server->erase(id);\n    }\n    \n    void SwMargUpdate::updateStateStereo(std::shared_ptr<State> state,\n                                         std::shared_ptr<MapServer> map_server,\n                                         std::shared_ptr<Triangulator> tri)\n    {\n        double marg_time = state->nextMargTime();\n        \n        if (marg_time == INFINITY) return;\n                \n        std::vector<double> selected_timestamps;\n        this->selectSwTimestamps(state->_sw_camleft_poses, marg_time, selected_timestamps);\n        \n        std::vector<std::shared_ptr<SE3>> sw_var_order;\n        std::map<std::shared_ptr<SE3>, int> sw_index_map;\n        std::vector<std::shared_ptr<Type>> sw_var_type;\n        \n        this->generateSwVarOrder(state, selected_timestamps, sw_var_order,\n                                 sw_index_map, sw_var_type);\n        \n        std::vector<int> update_ids;\n        \n        for (const auto& item : *map_server)\n        {\n            const auto& feat_info_ptr = item.second;\n            \n            if (feat_info_ptr->_ftype != FeatureInfo::FeatureType::MSCKF)\n                continue;\n            \n            bool flag = false;\n            const auto& stereo_obs = item.second->_stereo_obs;\n            \n            for (const double& ts : selected_timestamps)\n                if (stereo_obs.find(ts) == stereo_obs.end())\n                {\n                    flag = true;\n                    break;\n                }\n                \n            if (flag) continue;\n                \n            if (FeatureInfoManager::triangulateFeatureInfoStereo(feat_info_ptr, tri, state))\n                update_ids.push_back(item.first);\n        }\n        \n        if (update_ids.size() == 0) return;\n        \n        const int max_possible_cols = 6*state->_sw_camleft_poses.size();\n        \n        const int max_possible_rows = update_ids.size()*(4*selected_timestamps.size()-3);\n        \n        Eigen::VectorXd res_large(max_possible_rows);\n        Eigen::MatrixXd H_large(max_possible_rows, max_possible_cols);\n        res_large.setZero();\n        H_large.setZero();\n        \n        int row_cnt = 0;\n        int col_cnt = 6*sw_var_type.size();\n        \n        int feats_cnt = 0;\n        \n        for (int i = 0; i < update_ids.size(); ++i)\n        {\n            Eigen::VectorXd res_block;\n            Eigen::MatrixXd H_block;\n            Eigen::MatrixXd H_anchor_block;\n            \n            const auto& anchor_pose_ptr = this->calcResJacobianSingleFeatSelectedStereoObs(\n                map_server->at(update_ids[i]), state->_sw_camleft_poses,\n                sw_var_order, sw_index_map, selected_timestamps,\n                state->_state_params._T_cl2cr,\n                res_block, H_block, H_anchor_block);\n            \n            bool flag = false;\n            \n            if (sw_index_map.find(anchor_pose_ptr) == sw_index_map.end())\n            {\n                flag = true;\n                \n                sw_var_order.push_back(anchor_pose_ptr);\n                sw_var_type.push_back(anchor_pose_ptr);\n                sw_index_map[anchor_pose_ptr] = col_cnt;\n                \n                col_cnt += anchor_pose_ptr->size();\n                \n                H_block.conservativeResize(H_block.rows(), H_block.cols() + 6);\n            }\n            \n            H_block.block(0, sw_index_map.at(anchor_pose_ptr), H_block.rows(), 6) = H_anchor_block;\n            \n            if (!testChiSquared(state, res_block, H_block, sw_var_type, \n                this->_noise, selected_timestamps.size()-1))\n            {\n                if (flag)\n                {\n                    sw_var_order.pop_back();\n                    sw_var_type.pop_back();\n                    sw_index_map.erase(anchor_pose_ptr);\n                    \n                    col_cnt -= anchor_pose_ptr->size();\n                }\n                \n                continue;\n            }\n            \n            H_large.block(row_cnt, 0, H_block.rows(), col_cnt) = H_block;\n            res_large.block(row_cnt, 0, res_block.rows(), 1) = res_block;\n            \n            row_cnt += res_block.rows();\n            \n            ++feats_cnt;\n        }\n        \n        if (row_cnt < max_possible_rows || col_cnt < max_possible_cols)\n        {\n            H_large.conservativeResize(row_cnt, col_cnt);\n            res_large.conservativeResize(row_cnt, 1);\n        }\n        \n        Eigen::MatrixXd H_thin;\n        Eigen::VectorXd res_thin;\n        \n        if (H_large.rows() > H_large.cols())\n        {\n            Eigen::SparseMatrix<double> H_sparse = H_large.sparseView();\n            \n            Eigen::SPQR<Eigen::SparseMatrix<double>> spqr_helper;\n            spqr_helper.setSPQROrdering(SPQR_ORDERING_NATURAL);\n            spqr_helper.compute(H_sparse);\n            \n            Eigen::MatrixXd H_temp;\n            Eigen::VectorXd r_temp;\n            \n            (spqr_helper.matrixQ().transpose()*H_large).evalTo(H_temp);\n            (spqr_helper.matrixQ().transpose()*res_large).evalTo(r_temp);\n            \n            H_thin = H_temp.topRows(col_cnt);\n            res_thin = r_temp.head(col_cnt);                 \n        }\n        else\n        {\n            H_thin = H_large;\n            res_thin = res_large;\n        }\n        \n        /*\n        std::cout << \"[SwMargUpdate]: Num of stereo feats used in selected pose = \" << feats_cnt << std::endl;\n        */\n        \n        StateManager::ekfUpdate(state, sw_var_type, H_thin, res_thin, std::pow(this->_noise, 2)*Eigen::MatrixXd::Identity(res_thin.rows(), res_thin.rows()));\n        \n    }\n    \n    void SwMargUpdate::changeMSCKFAnchor(std::shared_ptr<State> state,\n                                         std::shared_ptr<MapServer> map_server)\n    {\n        double marg_time = state->nextMargTime();\n        \n        if (marg_time == INFINITY || state->_sw_camleft_poses.find(marg_time) == state->_sw_camleft_poses.end()) return;\n        \n        const std::shared_ptr<SE3> old_anchor = state->_sw_camleft_poses.at(marg_time);\n        \n        const double latest_sw = state->_sw_camleft_poses.rbegin()->first;\n        \n        const std::shared_ptr<SE3> new_anchor = state->_sw_camleft_poses.at(latest_sw);\n        \n        std::vector<int> ids_to_marg;\n        \n        for (auto& item : *map_server)\n        {\n            if (item.second->_ftype != FeatureInfo::FeatureType::MSCKF)\n                continue;\n                \n            if (item.second->_landmark->getAnchoredPose() == old_anchor)\n            {\n                \n                if (item.second->_isTri)\n                {\n                    const Eigen::Vector3d pf = item.second->_landmark->valuePosXyz();\n                    const Eigen::Vector3d body = new_anchor->valueLinearAsMat().transpose()*(pf - new_anchor->valueTrans());\n                    \n                    if (body.z() <= 0)\n                    {\n                        ids_to_marg.push_back(item.first);\n                        continue;\n                    }\n\n                    item.second->_landmark->resetAnchoredPose(new_anchor, true);\n                }\n                else\n                    ids_to_marg.push_back(item.first);\n            }\n        }\n        \n        for (const int& id: ids_to_marg)\n            map_server->erase(id);\n    }\n    \n    void SwMargUpdate::margSwPose(std::shared_ptr<State> state)\n    {\n        double marg_time = state->nextMargTime();\n        \n        if (marg_time == INFINITY) return;\n        \n        StateManager::margSlidingWindowPose(state, marg_time);\n    }\n    \n    void SwMargUpdate::cleanStereoObsAtMargTime(std::shared_ptr<State> state,\n                                  std::shared_ptr<MapServer> map_server)\n    {\n        double marg_time = state->nextMargTime();\n        \n        if (marg_time == INFINITY) return;\n        \n        std::vector<int> all_ids;\n        for (const auto& item : *map_server)\n            all_ids.push_back(item.first);\n        \n        std::vector<int> ids_to_clean;\n        for (int i = 0; i < all_ids.size(); ++i)\n        {\n            if (map_server->at(all_ids[i])->_stereo_obs.find(marg_time) != map_server->at(all_ids[i])->_stereo_obs.end())\n                map_server->at(all_ids[i])->_stereo_obs.erase(marg_time);\n            \n            if (map_server->at(all_ids[i])->_stereo_obs.size() == 0)\n                ids_to_clean.push_back(all_ids[i]);\n        }\n        \n        for (int id : ids_to_clean)\n            map_server->erase(id);\n    }\n    \n    void SwMargUpdate::generateSwVarOrder(const std::shared_ptr<State> state,\n                                          const std::vector<double>& selected_timestamps,\n                                          std::vector<std::shared_ptr<SE3>>& sw_var_order,\n                                          std::map<std::shared_ptr<SE3>, int>& sw_index,\n                                          std::vector<std::shared_ptr<Type>>& sw_var_type)\n    {\n        sw_var_order.clear();\n        sw_index.clear();\n        sw_var_type.clear();\n        \n        int cnt = 0;\n        \n        const auto& sw_poses = state->_sw_camleft_poses;\n        \n        for (const double& ts : selected_timestamps)\n        {\n            if (sw_poses.find(ts) == sw_poses.end())\n            {\n                std::cout << \"[SwMargUpdate]: selected timestamp not in sw!\" << std::endl;\n                std::exit(EXIT_FAILURE);\n            }\n            \n            sw_var_order.push_back(sw_poses.at(ts));\n            sw_var_type.push_back(sw_poses.at(ts));\n            sw_index[sw_poses.at(ts)] = 6*cnt;\n            ++cnt;\n        }\n    }\n    \n    void SwMargUpdate::selectSwTimestamps(\n        const std::map<double, std::shared_ptr<SE3>>& sw_poses,\n        const double& marg_time,\n        std::vector<double>& selected_timestamps)\n    {\n        selected_timestamps.clear();\n        \n        if (marg_time == INFINITY || sw_poses.find(marg_time) == sw_poses.end()) return;\n        \n        int cnt = 1;\n        selected_timestamps.push_back(marg_time);\n        \n        for (const auto& item : sw_poses)\n        {\n            if (item.first <= marg_time)\n                continue;\n            \n            if (cnt % _frame_select_interval == 0) selected_timestamps.push_back(item.first);\n            \n            ++cnt;\n        }\n    \n    }\n    \n    std::shared_ptr<SE3> SwMargUpdate::calcResJacobianSingleFeatSelectedMonoObs(\n        const std::shared_ptr<FeatureInfo> feature_info,\n        const std::map<double, std::shared_ptr<SE3>>& sw_poses,\n        const std::vector<std::shared_ptr<SE3>>& sw_var_order,\n        const std::map<std::shared_ptr<SE3>, int>& sw_index_map,\n        const std::vector<double>& selected_timestamps,\n        Eigen::VectorXd& res_block,\n        Eigen::MatrixXd& H_block,\n        Eigen::MatrixXd& H_anchor_block)\n    {\n        const int num_of_cols = 6*sw_var_order.size();\n        const int num_of_rows = 2*selected_timestamps.size();\n        \n        Eigen::VectorXd res_block_tmp = Eigen::VectorXd::Zero(num_of_rows);\n        Eigen::MatrixXd H_block_tmp = Eigen::MatrixXd::Zero(num_of_rows, num_of_cols);\n        Eigen::MatrixXd H_anchor_block_tmp = Eigen::MatrixXd::Zero(num_of_rows, 6);\n        Eigen::MatrixXd Hf_block_tmp = Eigen::MatrixXd::Zero(num_of_rows, 3);\n        \n        const Eigen::Vector3d pf_w = feature_info->_landmark->valuePosXyz();\n        const std::shared_ptr<SE3> pose_anchor_ptr = feature_info->_landmark->getAnchoredPose();\n        \n        int row_cnt = 0;\n        \n        for (const auto& time_of_obs : selected_timestamps)\n        {\n            if (sw_poses.find(time_of_obs) == sw_poses.end() || feature_info->_mono_obs.find(time_of_obs) == feature_info->_mono_obs.end())\n                continue;\n            \n            const std::shared_ptr<MonoMeas>& mono_obs_ptr = feature_info->_mono_obs.at(time_of_obs);\n            const std::shared_ptr<SE3>& pose_obs_ptr = sw_poses.at(time_of_obs);\n            \n    \n            const Eigen::Matrix3d R_cm2w = pose_obs_ptr->valueLinearAsMat();\n            const Eigen::Vector3d p_cm = pose_obs_ptr->valueTrans();\n            \n            const Eigen::Vector3d pf_cm = R_cm2w.transpose()*(pf_w-p_cm);\n            \n            Eigen::Matrix<double, 2, 3> H_proj = Eigen::Matrix<double, 2, 3>::Zero();\n            H_proj(0, 0) = 1.0/pf_cm.z();\n            H_proj(0, 2) = -pf_cm.x()/std::pow(pf_cm.z(), 2);\n            H_proj(1, 1) = 1.0/pf_cm.z();\n            H_proj(1, 2) = -pf_cm.y()/std::pow(pf_cm.z(), 2);\n            \n            Eigen::MatrixXd H_pf2x = Eigen::MatrixXd::Zero(3, num_of_cols);\n            Eigen::Matrix<double, 3, 6> H_pf2anchor = Eigen::Matrix<double, 3, 6>::Zero();\n            \n            if (pose_obs_ptr != pose_anchor_ptr)\n            {\n                H_pf2x.block<3, 3>(0, sw_index_map.at(pose_obs_ptr)) = R_cm2w.transpose()*skew(pf_w);\n            \n                H_pf2anchor.block<3, 3>(0, 0) = -H_pf2x.block<3, 3>(0, sw_index_map.at(pose_obs_ptr));\n            }\n            \n            H_pf2x.block<3, 3>(0, sw_index_map.at(pose_obs_ptr)+3) = -R_cm2w.transpose();\n            Eigen::Matrix3d H_pf2pf = R_cm2w.transpose();\n            \n            if (H_proj.hasNaN() || H_pf2x.hasNaN()) \n                continue;\n            \n            H_block_tmp.block(row_cnt, 0, 2, num_of_cols) = H_proj*H_pf2x;\n            \n            H_anchor_block_tmp.block(row_cnt, 0, 2, 6) = H_proj*H_pf2anchor;\n            \n            Hf_block_tmp.block(row_cnt, 0, 2, 3) = H_proj*H_pf2pf;\n            \n            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());\n            \n            row_cnt += 2;\n        }\n        \n        if (row_cnt < res_block.rows())\n        {\n            res_block_tmp.conservativeResize(row_cnt, 1);\n            H_block_tmp.conservativeResize(row_cnt, num_of_cols);\n            H_anchor_block_tmp.conservativeResize(row_cnt, 6);\n            Hf_block_tmp.conservativeResize(row_cnt, 3);\n        }\n        \n        if (row_cnt <= 3)\n            std::cout << \"[SwMargUpdate]: Warning! in calc one feat selected jacobian, num of rows <= 3 !\" << std::endl;\n        \n        Eigen::JacobiSVD<Eigen::MatrixXd> svd_helper(Hf_block_tmp, Eigen::ComputeFullU | Eigen::ComputeThinV);\n        Eigen::MatrixXd V = svd_helper.matrixU().rightCols(row_cnt-3);\n        \n        H_block = V.transpose()*H_block_tmp;\n        res_block = V.transpose()*res_block_tmp;\n        H_anchor_block = V.transpose()*H_anchor_block_tmp;\n        \n        return pose_anchor_ptr;\n    }\n    \n    std::shared_ptr<SE3> SwMargUpdate::calcResJacobianSingleFeatSelectedStereoObs(\n        const std::shared_ptr<FeatureInfo> feature_info, \n        const std::map<double, std::shared_ptr<SE3>>& sw_poses,\n        const std::vector<std::shared_ptr<SE3>>& sw_var_order,\n        const std::map<std::shared_ptr<SE3>, int>& sw_index_map,\n        const std::vector<double>& selected_timestamps,\n        const Eigen::Isometry3d& T_cl2cr,\n        Eigen::VectorXd& res_block,\n        Eigen::MatrixXd& H_block,\n        Eigen::MatrixXd& H_anchor_block)\n    {\n        \n        const Eigen::Matrix3d& R_cl2cr = T_cl2cr.linear();\n        \n        const int num_of_cols = 6*sw_var_order.size();\n        const int num_of_rows = 4*selected_timestamps.size();\n        \n        Eigen::VectorXd res_block_tmp = Eigen::VectorXd::Zero(num_of_rows);\n        Eigen::MatrixXd H_block_tmp = Eigen::MatrixXd::Zero(num_of_rows, num_of_cols);\n        Eigen::MatrixXd H_anchor_block_tmp = Eigen::MatrixXd::Zero(num_of_rows, 6);\n        Eigen::MatrixXd Hf_block_tmp = Eigen::MatrixXd::Zero(num_of_rows, 3);\n        \n        const Eigen::Vector3d pf_w = feature_info->_landmark->valuePosXyz();\n        const std::shared_ptr<SE3> pose_anchor_ptr = feature_info->_landmark->getAnchoredPose();\n        \n        int row_cnt = 0;\n        \n        for (const auto& time_of_obs : selected_timestamps)\n        {\n            if (sw_poses.find(time_of_obs) == sw_poses.end() || feature_info->_stereo_obs.find(time_of_obs) == feature_info->_stereo_obs.end())\n                continue;\n            \n            const std::shared_ptr<StereoMeas>& stereo_obs_ptr = feature_info->_stereo_obs.at(time_of_obs);\n            const std::shared_ptr<SE3>& pose_obs_ptr = sw_poses.at(time_of_obs);\n            \n            const Eigen::Matrix3d R_cm2w = pose_obs_ptr->valueLinearAsMat();\n            const Eigen::Vector3d p_cm = pose_obs_ptr->valueTrans();\n            \n            const Eigen::Vector3d pf_cm = R_cm2w.transpose()*(pf_w-p_cm);\n            \n            Eigen::Matrix<double, 2, 3> H_proj = Eigen::Matrix<double, 2, 3>::Zero();\n            H_proj(0, 0) = 1.0/pf_cm.z();\n            H_proj(0, 2) = -pf_cm.x()/std::pow(pf_cm.z(), 2);\n            H_proj(1, 1) = 1.0/pf_cm.z();\n            H_proj(1, 2) = -pf_cm.y()/std::pow(pf_cm.z(), 2);\n            \n            const Eigen::Vector3d pf_cm_r = T_cl2cr*pf_cm;\n            \n            Eigen::Matrix<double, 2, 3> H_proj_r = Eigen::Matrix<double, 2, 3>::Zero();\n            H_proj_r(0, 0) = 1.0/pf_cm_r.z();\n            H_proj_r(0, 2) = -pf_cm_r.x()/std::pow(pf_cm_r.z(), 2);\n            H_proj_r(1, 1) = 1.0/pf_cm_r.z();\n            H_proj_r(1, 2) = -pf_cm_r.y()/std::pow(pf_cm_r.z(), 2);\n            \n            Eigen::MatrixXd H_pf2x = Eigen::MatrixXd::Zero(3, num_of_cols);\n            Eigen::Matrix<double, 3, 6> H_pf2anchor = Eigen::Matrix<double, 3, 6>::Zero();\n            \n            if (pose_obs_ptr != pose_anchor_ptr)\n            {\n                H_pf2x.block<3, 3>(0, sw_index_map.at(pose_obs_ptr)) = R_cm2w.transpose()*skew(pf_w);\n            \n                H_pf2anchor.block<3, 3>(0, 0) = -H_pf2x.block<3, 3>(0, sw_index_map.at(pose_obs_ptr));\n            }\n            \n            H_pf2x.block<3, 3>(0, sw_index_map.at(pose_obs_ptr)+3) = -R_cm2w.transpose();\n            Eigen::Matrix3d H_pf2pf = R_cm2w.transpose();\n            \n            Eigen::MatrixXd H_pf2x_r = R_cl2cr*H_pf2x;\n            Eigen::Matrix<double, 3, 6> H_pf2anchor_r = R_cl2cr*H_pf2anchor;\n            Eigen::MatrixXd H_pf2pf_r = R_cl2cr*H_pf2pf;\n            \n            if (H_proj.hasNaN() || H_pf2x.hasNaN()) \n                continue;\n            \n            H_block_tmp.block(row_cnt, 0, 2, num_of_cols) = H_proj*H_pf2x;\n            \n            H_anchor_block_tmp.block(row_cnt, 0, 2, 6) = H_proj*H_pf2anchor;\n            \n            Hf_block_tmp.block(row_cnt, 0, 2, 3) = H_proj*H_pf2pf;\n            \n            H_block_tmp.block(row_cnt+2, 0, 2, num_of_cols) = H_proj_r*H_pf2x_r;\n            \n            H_anchor_block_tmp.block(row_cnt+2, 0, 2, 6) = H_proj_r*H_pf2anchor_r;\n            \n            Hf_block_tmp.block(row_cnt+2, 0, 2, 3) = H_proj_r*H_pf2pf_r;\n            \n            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());\n            \n            row_cnt += 4;\n        }\n        \n        if (row_cnt < res_block.rows())\n        {\n            res_block_tmp.conservativeResize(row_cnt, 1);\n            H_block_tmp.conservativeResize(row_cnt, num_of_cols);\n            H_anchor_block_tmp.conservativeResize(row_cnt, 6);\n            Hf_block_tmp.conservativeResize(row_cnt, 3);\n        }\n        \n        if (row_cnt <= 3)\n            std::cout << \"[SwMargUpdate]: Warning! in calc one feat selected jacobian, num of rows <= 3 !\" << std::endl;\n        \n        Eigen::JacobiSVD<Eigen::MatrixXd> svd_helper(Hf_block_tmp, Eigen::ComputeFullU | Eigen::ComputeThinV);\n        Eigen::MatrixXd V = svd_helper.matrixU().rightCols(row_cnt-3);\n        \n        H_block = V.transpose()*H_block_tmp;\n        H_anchor_block = V.transpose()*H_anchor_block_tmp;\n        res_block = V.transpose()*res_block_tmp;\n        \n        return pose_anchor_ptr;\n    }\n    \n    void SwMargUpdate::removeMonoMSCKFinMargPose(std::shared_ptr<State> state,\n                                             std::shared_ptr<MapServer> map_server)\n    {\n        const double marg_time = state->nextMargTime();\n        \n        if (marg_time == INFINITY) return;\n        \n        std::vector<int> ids_to_marg;\n        \n        for (const auto& item : *map_server)\n            if (item.second->_mono_obs.find(marg_time) != item.second->_mono_obs.end() &&\n                item.second->_ftype == FeatureInfo::FeatureType::MSCKF)\n                ids_to_marg.push_back(item.first);\n        \n        for (const int& id : ids_to_marg)\n            map_server->erase(id);\n    }\n    \n    void SwMargUpdate::removeStereoMSCKFinMargPose(std::shared_ptr<State> state,\n                                                 std::shared_ptr<MapServer> map_server)\n    {\n        const double marg_time = state->nextMargTime();\n        \n        if (marg_time == INFINITY) return;\n        \n        std::vector<int> ids_to_marg;\n        \n        for (const auto& item : *map_server)\n            if (item.second->_stereo_obs.find(marg_time) != item.second->_stereo_obs.end() && \n                item.second->_ftype == FeatureInfo::FeatureType::MSCKF)\n                ids_to_marg.push_back(item.first);\n            \n            for (const int& id : ids_to_marg)\n                map_server->erase(id);\n    }\n}\n"
  },
  {
    "path": "ingvio_estimator/src/SwMargUpdate.h",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#pragma once\n\n#include \"MapServer.h\"\n\n#include \"Update.h\"\n\nnamespace ingvio\n{\n    class Triangulator;\n    class IngvioParams;\n    \n    class SwMargUpdate : public UpdateBase\n    {\n    public:\n        using UpdateBase::UpdateBase;\n        \n        SwMargUpdate(const IngvioParams& filter_params) : \n        UpdateBase(filter_params._chi2_max_dof, filter_params._chi2_thres),\n        _noise(filter_params._visual_noise),\n        _frame_select_interval(filter_params._frame_select_interval)\n        {}\n        \n        SwMargUpdate(const SwMargUpdate&) = delete;\n        SwMargUpdate operator=(const SwMargUpdate&) = delete;\n        \n        virtual ~SwMargUpdate() {}\n        \n        void updateStateMono(std::shared_ptr<State> state,\n                             std::shared_ptr<MapServer> map_server,\n                             std::shared_ptr<Triangulator> tri);\n        \n        void cleanMonoObsAtMargTime(std::shared_ptr<State> state,\n                                    std::shared_ptr<MapServer> map_server);\n        \n        void updateStateStereo(std::shared_ptr<State> state,\n                               std::shared_ptr<MapServer> map_server,\n                               std::shared_ptr<Triangulator> tri);\n        \n        void cleanStereoObsAtMargTime(std::shared_ptr<State> state,\n                                      std::shared_ptr<MapServer> map_server);\n        \n        void changeMSCKFAnchor(std::shared_ptr<State> state,\n                               std::shared_ptr<MapServer> map_server);\n        \n        void margSwPose(std::shared_ptr<State> state);\n        \n        void removeMonoMSCKFinMargPose(std::shared_ptr<State> state,\n                                   std::shared_ptr<MapServer> map_server);\n        \n        void removeStereoMSCKFinMargPose(std::shared_ptr<State> state,\n                                         std::shared_ptr<MapServer> map_server);\n        \n    protected:\n        \n        double _noise;\n        \n        int _frame_select_interval;\n        \n        void generateSwVarOrder(const std::shared_ptr<State> state,\n                                const std::vector<double>& selected_timestamps,\n                                std::vector<std::shared_ptr<SE3>>& sw_var_order,\n                                std::map<std::shared_ptr<SE3>, int>& sw_index,\n                                std::vector<std::shared_ptr<Type>>& sw_var_type);\n        \n        std::shared_ptr<SE3> calcResJacobianSingleFeatSelectedMonoObs(\n            const std::shared_ptr<FeatureInfo> feature_info,\n            const std::map<double, std::shared_ptr<SE3>>& sw_poses,\n            const std::vector<std::shared_ptr<SE3>>& sw_var_order,\n            const std::map<std::shared_ptr<SE3>, int>& sw_index_map,\n            const std::vector<double>& selected_timestamps,\n            Eigen::VectorXd& res_block,\n            Eigen::MatrixXd& H_block,\n            Eigen::MatrixXd& H_anchor_block);\n        \n        std::shared_ptr<SE3> calcResJacobianSingleFeatSelectedStereoObs(\n            const std::shared_ptr<FeatureInfo> feature_info, \n            const std::map<double, std::shared_ptr<SE3>>& sw_poses,\n            const std::vector<std::shared_ptr<SE3>>& sw_var_order,\n            const std::map<std::shared_ptr<SE3>, int>& sw_index_map,\n            const std::vector<double>& selected_timestamps,\n            const Eigen::Isometry3d& T_cl2cr,\n            Eigen::VectorXd& res_block,\n            Eigen::MatrixXd& H_block,\n            Eigen::MatrixXd& H_anchor_block);\n        \n        void selectSwTimestamps(const std::map<double, std::shared_ptr<SE3>>& sw_poses,\n                                const double& marg_time,\n                                std::vector<double>& selected_timestamps);\n    };\n}\n"
  },
  {
    "path": "ingvio_estimator/src/TicToc.h",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n// The TicToc Class is inspired by such realization in GVINS.\n// see https://github.com/HKUST-Aerial-Robotics/GVINS\n\n#pragma once\n\n#include <ctime>\n#include <cstdlib>\n#include <chrono>\n\nnamespace ingvio\n{\n    class TicToc\n    {\n    public:\n        TicToc()\n        {\n            tic();\n        }\n        \n        void tic()\n        {\n            start = std::chrono::system_clock::now();\n        }\n        \n        double toc()\n        {\n            end = std::chrono::system_clock::now();\n            \n            std::chrono::duration<double> elapsed_seconds = end - start;\n            \n            return elapsed_seconds.count() * 1000;\n        }\n        \n    private:\n        std::chrono::time_point<std::chrono::system_clock> start, end;\n    };\n}\n"
  },
  {
    "path": "ingvio_estimator/src/Triangulator.cpp",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#include <Eigen/QR>\n\n#include \"Triangulator.h\"\n\n#include \"MapServer.h\"\n#include \"PoseState.h\"\n\nnamespace ingvio\n{\n    double Triangulator::findLongestTrans(\n        const std::map<double, std::shared_ptr<SE3>, std::less<double>>& sw_poses,\n        const std::map<double, std::shared_ptr<MonoMeas>>& mobs,\n        double& max_length) const\n    {\n        assert(sw_poses.size() >= 2);\n        \n        const auto& item_last = sw_poses.rbegin();\n        const double timestamp_last = item_last->first;\n        const std::shared_ptr<SE3> pose_last = item_last->second;\n        \n        Eigen::Vector3d unit_feat_last;\n        unit_feat_last.head<2>() = mobs.at(timestamp_last)->asVec();\n        unit_feat_last.z() = 1.0;\n        unit_feat_last.normalize();\n        \n        const Eigen::Vector3d unit_feat_w = pose_last->valueLinearAsMat()*unit_feat_last;\n        const Eigen::Matrix3d proj = Eigen::Matrix3d::Identity()-unit_feat_w*unit_feat_w.transpose();\n        \n        max_length = -INFINITY;\n        double max_timestamp = timestamp_last;\n        for (const auto& item : sw_poses)\n        {\n            if (item.first == timestamp_last) continue;\n            \n            const Eigen::Vector3d trans_vec = proj*(item.second->valueTrans()-pose_last->valueTrans());\n            \n            double trans = std::abs(trans_vec.norm());\n            if (trans > max_length)\n            {\n                max_length = trans;\n                max_timestamp = item.first;\n            }\n        }\n        \n        return max_timestamp;\n    }\n    \n    void Triangulator::calcRelaSwPose(\n        const std::map<double, std::shared_ptr<SE3>, std::less<double>>& sw_poses,\n        std::map<double, std::shared_ptr<SE3>, std::less<double>>& rel_sw_poses) const\n    {\n        const std::shared_ptr<SE3> pose_last = sw_poses.rbegin()->second;\n        const double timestamp_last = sw_poses.rbegin()->first;\n        \n        rel_sw_poses.clear();\n        for (const auto& item : sw_poses)\n        {\n            rel_sw_poses[item.first] = std::make_shared<SE3>();\n            \n            if (item.first == timestamp_last)\n            {\n                rel_sw_poses[timestamp_last]->setIdentity();\n                continue;\n            }\n            rel_sw_poses[item.first]->setValueByIso(item.second->copyValueAsIso().inverse()*pose_last->copyValueAsIso());\n        }\n    }\n    \n    double Triangulator::initDepth(const Eigen::Vector2d& m1,\n                                   const Eigen::Vector2d& m2,\n                                   const std::shared_ptr<SE3> T12) const\n    {\n        Eigen::Vector3d tilde_m1 = T12->valueLinearAsMat()*Eigen::Vector3d(m1.x(), m1.y(), 1.0);\n        Eigen::Vector3d t = T12->valueTrans();\n        \n        Eigen::Vector2d A, b;\n        \n        A(0, 0) = tilde_m1.x() - m2.x()*tilde_m1.z();\n        A(1, 0) = tilde_m1.y() - m2.y()*tilde_m1.z();\n        \n        b(0, 0) = m2.x()*t.z()-t.x();\n        b(1, 0) = m2.y()*t.z()-t.y();\n        \n        return (A.transpose()*A).inverse()*A.transpose()*b;\n    }\n    \n    double Triangulator::calcUnitCost(const Eigen::Vector2d& meas,\n                                      const std::shared_ptr<SE3> rel_pose,\n                                      const Eigen::Vector3d& solution) const\n    {\n        Eigen::Vector3d pf0;\n        \n        pf0.z() = 1.0/solution.z();\n        pf0.x() = solution.x()*pf0.z();\n        pf0.y() = solution.y()*pf0.z();\n        \n        Eigen::Vector3d pf = rel_pose->valueLinearAsMat()*pf0 + rel_pose->valueTrans();\n        \n        Eigen::Vector2d meas_hat;\n        meas_hat.x() = pf.x()/pf.z();\n        meas_hat.y() = pf.y()/pf.z();\n        \n        return (meas-meas_hat).squaredNorm();\n    }\n    \n    double Triangulator::calcTotalCost(const std::map<double, std::shared_ptr<MonoMeas>>& mobs,\n                                       const std::map<double, std::shared_ptr<SE3>>& rel_poses,\n                                       const Eigen::Vector3d& solution) const\n    {\n        double total_cost = 0.0;\n        \n        for (const auto& item : mobs)\n            total_cost += calcUnitCost(item.second->asVec(), rel_poses.at(item.first), solution);\n        \n        return total_cost;\n    }\n    \n    void Triangulator::calcResJacobian(const Eigen::Vector2d& meas,\n                                       const std::shared_ptr<SE3> rel_pose,\n                                       const Eigen::Vector3d& solution,\n                                       Eigen::Vector2d& res,\n                                       Eigen::Matrix<double, 2, 3>& J,\n                                       double& w) const\n    {\n        Eigen::Vector3d tilde_pf_hat = rel_pose->valueLinearAsMat()*Eigen::Vector3d(solution.x(), solution.y(), 1.0)+rel_pose->valueTrans()*solution.z();\n        \n        Eigen::Vector2d meas_hat;\n        meas_hat.x() = tilde_pf_hat.x()/tilde_pf_hat.z();\n        meas_hat.y() = tilde_pf_hat.y()/tilde_pf_hat.z();\n        \n        res = meas_hat - meas;\n        \n        Eigen::Matrix<double, 2, 3> W;\n        W.setZero();\n        W(0, 0) = 1.0/tilde_pf_hat.z(); \n        W(0, 2) = -tilde_pf_hat.x()/std::pow(tilde_pf_hat.z(), 2);\n        W(1, 1) = 1.0/tilde_pf_hat.z();\n        W(1, 2) = -tilde_pf_hat.y()/std::pow(tilde_pf_hat.z(), 2);\n        \n        Eigen::Matrix<double, 3, 3> U;\n        U.rightCols<1>() = rel_pose->valueTrans();\n        U.leftCols<2>() = rel_pose->valueLinearAsMat().leftCols<2>();\n        \n        J = W*U;\n        \n        double e = res.norm();\n        if (e <= _huber_epsilon)\n            w = 1.0;\n        else\n            w = std::sqrt(2.0*_huber_epsilon/e);\n    }\n    \n    bool Triangulator::triangulateMonoObs(\n        const std::map<double, std::shared_ptr<MonoMeas>>& mono_obs,\n        const std::map<double, std::shared_ptr<SE3>, std::less<double>>& sw_poses_raw,\n        Eigen::Vector3d& pf) const\n    {\n        std::map<double, std::shared_ptr<MonoMeas>> mobs;\n        std::map<double, std::shared_ptr<SE3>, std::less<double>> sw_poses;\n        \n        this->filterCommonTimestamp<MonoMeas>(sw_poses_raw, mono_obs, sw_poses, mobs);\n        \n        if (mobs.size() <= 4)\n        {\n            pf.setZero();\n            return false;\n        }\n        \n        double max_trans;\n        double max_timestamp = findLongestTrans(sw_poses, mobs, max_trans);\n        \n        if (max_trans < _trans_thres) return false;\n            \n        std::map<double, std::shared_ptr<SE3>> rel_sw_poses;\n        calcRelaSwPose(sw_poses, rel_sw_poses);\n        \n        Eigen::Vector3d solution;\n        const double timestamp_last = rel_sw_poses.rbegin()->first;\n        const std::shared_ptr<SE3> pose_last = sw_poses.rbegin()->second;\n        \n        solution.x() = mobs[timestamp_last]->_u0;\n        solution.y() = mobs[timestamp_last]->_v0;\n        solution.z() = 1.0/initDepth(mobs[timestamp_last]->asVec(), mobs[max_timestamp]->asVec(), rel_sw_poses[max_timestamp]);\n        \n        /*\n        std::cout << \"init pose = \" << (pose0->copyValueAsIso().inverse()*Eigen::Vector3d(solution.x()/solution.z(), solution.y()/solution.z(), 1.0/solution.z())).transpose() << std::endl;\n        */\n        \n        double total_cost = calcTotalCost(mobs, rel_sw_poses, solution);\n        \n        double lambda = _init_damping;\n        int inner_loop_cnt = 0;\n        int outer_loop_cnt = 0;\n        bool is_cost_reduced = false;\n        double delta_norm = INFINITY;\n        \n        do\n        {\n            Eigen::Matrix3d A = Eigen::Matrix3d::Zero();\n            Eigen::Vector3d b = Eigen::Vector3d::Zero();\n            \n            for (const auto& item : rel_sw_poses)\n            {\n                Eigen::Matrix<double, 2, 3> J;\n                Eigen::Vector2d res;\n                double w;\n                \n                calcResJacobian(mobs.at(item.first)->asVec(), item.second, solution, res, J, w);\n                \n                if (w == 1.0)\n                {\n                    A += J.transpose()*J;\n                    b -= J.transpose()*res;\n                }\n                else\n                {\n                    A += std::pow(w, 2)*J.transpose()*J;\n                    b -= std::pow(w, 2)*J.transpose()*res;\n                }\n            }\n            \n            do\n            {\n                Eigen::Matrix3d Damper = lambda*Eigen::Matrix3d::Identity();\n                \n                Eigen::Vector3d delta = (A + Damper).ldlt().solve(b);\n                \n                Eigen::Vector3d new_solution = solution + delta;\n                \n                delta_norm = delta.norm();\n                \n                double new_total_cost = calcTotalCost(mobs, rel_sw_poses, new_solution);\n                \n                \n                if (new_total_cost < total_cost)\n                {\n                    total_cost = new_total_cost;\n                    solution = new_solution;\n                    is_cost_reduced = true;\n                    lambda = lambda/10.0 > 1e-10 ? lambda/10.0 : 1e-10;\n                }\n                else\n                {\n                    is_cost_reduced = false;\n                    lambda = lambda*10 < 1e12 ? lambda*10 : 1e12;\n                }\n            }\n            while (inner_loop_cnt++ < _inner_loop_max_iter && !is_cost_reduced);\n            \n            inner_loop_cnt = 0;\n        }\n        while (outer_loop_cnt++ < _outer_loop_max_iter && delta_norm > _conv_precision);\n        \n        Eigen::Vector3d pf_last;\n        pf_last.z() = 1.0/solution.z();\n        pf_last.x() = solution.x()*pf_last.z();\n        pf_last.y() = solution.y()*pf_last.z();\n        \n        pf.setZero();\n        \n        if ((outer_loop_cnt >= _outer_loop_max_iter && inner_loop_cnt >= _inner_loop_max_iter) || delta_norm > _conv_precision) \n            return false;\n        \n        for (const auto& item : rel_sw_poses)\n        {\n            Eigen::Vector3d tmp = item.second->copyValueAsIso()*pf_last;\n            if (tmp.z() <= _min_depth)\n                return false;\n        }\n        \n        Eigen::HouseholderQR<Eigen::MatrixXd> qr(pf_last);\n        Eigen::MatrixXd Q = qr.householderQ();\n        \n        double base_line_max = 0.0;\n        \n        for (const auto& item : rel_sw_poses)\n        {\n            Eigen::Vector3d p_ci_A = item.second->copyValueAsIso().inverse().translation();\n            \n            double base_line = ((Q.block(0, 1, 3, 2)).transpose()*p_ci_A).norm();\n            \n            if (base_line > base_line_max)\n                base_line_max = base_line;\n        }\n        \n        if (pf_last.z() < _min_depth || pf_last.z() > _max_depth)\n            return false;\n        \n        pf = pose_last->copyValueAsIso()*pf_last;\n        \n        if (pf.hasNaN())\n        {\n            pf.setZero();\n            return false;\n        }\n        \n        return true;\n    }\n    \n    bool Triangulator::triangulateStereoObs(\n        const std::map<double, std::shared_ptr<StereoMeas>>& stereo_obs,\n        const std::map<double, std::shared_ptr<SE3>, std::less<double>>& sw_poses_raw,\n        const Eigen::Isometry3d& T_cl2cr,\n        Eigen::Vector3d& pf) const\n    {\n        std::map<double, std::shared_ptr<StereoMeas>> sobs;\n        std::map<double, std::shared_ptr<SE3>, std::less<double>> sw_poses;\n        \n        this->filterCommonTimestamp<StereoMeas>(sw_poses_raw, stereo_obs, sw_poses, sobs);\n        \n        std::map<double, std::shared_ptr<MonoMeas>> mono_obs;\n        std::map<double, std::shared_ptr<SE3>, std::less<double>> sw_mono_poses;\n        \n        double cnt = 0;\n        for (const auto& item : sobs)\n        {\n            cnt = cnt + 1.0;\n            mono_obs[cnt] = std::make_shared<MonoMeas>();\n            \n            mono_obs.at(cnt)->_u0 = item.second->_u0;\n            mono_obs.at(cnt)->_v0 = item.second->_v0;\n            \n            sw_mono_poses[cnt] = sw_poses.at(item.first)->clone();\n            \n            cnt = cnt + 1.0;\n            mono_obs[cnt] = std::make_shared<MonoMeas>();\n            \n            mono_obs.at(cnt)->_u0 = item.second->_u1;\n            mono_obs.at(cnt)->_v0 = item.second->_v1;\n            \n            sw_mono_poses[cnt] = std::make_shared<SE3>();\n            \n            sw_mono_poses.at(cnt)->setValueByIso(sw_poses.at(item.first)->copyValueAsIso()*T_cl2cr.inverse());\n        }\n        \n        bool flag = this->triangulateMonoObs(mono_obs, sw_mono_poses, pf);\n        \n        return flag;\n    }\n    \n}\n"
  },
  {
    "path": "ingvio_estimator/src/Triangulator.h",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#pragma once\n\n#include <map>\n#include <memory>\n\n#include \"IngvioParams.h\"\n\nnamespace ingvio\n{\n    struct MonoMeas;\n    struct StereoMeas;\n    \n    class SE3;\n    \n    class Triangulator\n    {\n    public:\n        Triangulator() {}\n        Triangulator(const IngvioParams& filter_params)\n        {\n            _trans_thres = filter_params._trans_thres;\n            _huber_epsilon = filter_params._huber_epsilon;\n            _conv_precision = filter_params._conv_precision;\n            _init_damping = filter_params._init_damping;\n            _outer_loop_max_iter = filter_params._outer_loop_max_iter;\n            _inner_loop_max_iter = filter_params._inner_loop_max_iter;\n            _max_depth = filter_params._max_depth;\n            _min_depth = filter_params._min_depth;\n            _max_baseline_ratio = filter_params._max_baseline_ratio;\n        }\n        \n        ~Triangulator() {}\n        \n        bool triangulateMonoObs(\n            const std::map<double, std::shared_ptr<MonoMeas>>& mono_obs,\n            const std::map<double, std::shared_ptr<SE3>, std::less<double>>& sw_poses_raw,\n            Eigen::Vector3d& pf) const;\n        \n        bool triangulateStereoObs(\n            const std::map<double, std::shared_ptr<StereoMeas>>& stereo_obs,\n            const std::map<double, std::shared_ptr<SE3>, std::less<double>>& sw_poses_raw,\n            const Eigen::Isometry3d& T_cl2cr,\n            Eigen::Vector3d& pf) const;\n        \n    protected:\n    \n        double _trans_thres = 0.1;\n        double _huber_epsilon = 0.01;\n        double _conv_precision = 5e-7;\n        double _init_damping = 1e-3;\n        int _outer_loop_max_iter = 10;\n        int _inner_loop_max_iter = 10;\n        \n        double _max_depth = 60.0;\n        double _min_depth = 0.2;\n        double _max_baseline_ratio = 40.0;\n        \n        double findLongestTrans(\n            const std::map<double, std::shared_ptr<SE3>, std::less<double>>& sw_poses,\n            const std::map<double, std::shared_ptr<MonoMeas>>& mobs,\n            double& max_length) const;\n        \n        double initDepth(const Eigen::Vector2d& m1,\n                         const Eigen::Vector2d& m2,\n                         const std::shared_ptr<SE3> T12) const;\n        \n        void calcRelaSwPose(\n            const std::map<double, std::shared_ptr<SE3>, std::less<double>>& sw_poses,\n            std::map<double, std::shared_ptr<SE3>, std::less<double>>& rel_sw_poses) const;\n        \n        double calcUnitCost(const Eigen::Vector2d& meas,\n                            const std::shared_ptr<SE3> rel_pose,\n                            const Eigen::Vector3d& solution) const;\n        \n        double calcTotalCost(const std::map<double, std::shared_ptr<MonoMeas>>& mobs,\n                             const std::map<double, std::shared_ptr<SE3>>& rel_poses,\n                             const Eigen::Vector3d& solution) const;\n        \n        void calcResJacobian(const Eigen::Vector2d& meas,\n                             const std::shared_ptr<SE3> rel_pose,\n                             const Eigen::Vector3d& solution,\n                             Eigen::Vector2d& res,\n                             Eigen::Matrix<double, 2, 3>& J,\n                             double& w) const;\n        \n        template<typename T>\n        void filterCommonTimestamp(\n            const std::map<double, std::shared_ptr<SE3>, std::less<double>>& poses,\n            const std::map<double, std::shared_ptr<T>>& obs,\n            std::map<double, std::shared_ptr<SE3>>& common_poses,\n            std::map<double, std::shared_ptr<T>>& common_obs) const\n        {\n            common_obs.clear();\n            common_poses.clear();\n            \n            for (const auto& item : obs)\n            {\n                if (poses.find(item.first) == poses.end()) continue;\n                common_obs[item.first] = item.second;\n                common_poses[item.first] = poses.at(item.first);\n            }\n        }\n    };\n}\n"
  },
  {
    "path": "ingvio_estimator/src/Update.cpp",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#include \"StateManager.h\"\n\n#include \"Update.h\"\n\nnamespace ingvio\n{\n    void UpdateBase::setChiSquaredTable(const int& max_dof, const double& thres)\n    {\n        for (int i = 1; i <= max_dof; ++i)\n        {\n            boost::math::chi_squared chi_squared_dist(i);\n            this->_chi_squared_table[i] = boost::math::quantile(chi_squared_dist, thres);\n        }\n    }\n    \n    double UpdateBase::whitenResidual(const std::shared_ptr<State> state,\n                                      const Eigen::VectorXd& res,\n                                      const Eigen::MatrixXd& H,\n                                      const std::vector<std::shared_ptr<Type>>& var_order,\n                                      double noise)\n    {\n        assert(StateManager::checkSubOrder(state, var_order));\n        \n        int idx = 0;\n        for (int i = 0; i < var_order.size(); ++i)\n            idx += var_order[i]->size();\n        \n        assert(res.rows() == H.rows());\n        assert(idx == H.cols());\n        \n        Eigen::MatrixXd small_cov = StateManager::getMarginalCov(state, var_order);\n        \n        Eigen::MatrixXd S = H*small_cov*H.transpose() + std::pow(noise, 2.0)*Eigen::MatrixXd::Identity(H.rows(), H.rows());\n        \n        return res.transpose()*S.ldlt().solve(res);\n    }\n    \n    double UpdateBase::whitenResidual(const std::shared_ptr<State> state,\n                                      const Eigen::VectorXd& res,\n                                      const Eigen::MatrixXd& H,\n                                      const std::vector<std::shared_ptr<Type>>& var_order,\n                                      const Eigen::MatrixXd& R)\n    {\n        assert(StateManager::checkSubOrder(state, var_order));\n        assert(R.rows() == R.cols());\n        \n        int idx = 0;\n        for (int i = 0; i < var_order.size(); ++i)\n            idx += var_order[i]->size();\n        \n        assert(res.rows() == H.rows());\n        assert(idx == H.cols());\n        \n        Eigen::MatrixXd small_cov = StateManager::getMarginalCov(state, var_order);\n        \n        Eigen::MatrixXd S = H*small_cov*H.transpose() + R;\n        \n        return res.transpose()*S.ldlt().solve(res);\n    }\n    \n    bool UpdateBase::testChiSquared(const std::shared_ptr<State> state,\n                                    const Eigen::VectorXd& res,\n                                    const Eigen::MatrixXd& H,\n                                    const std::vector<std::shared_ptr<Type>>& var_order,\n                                    double noise)\n    {\n        double prob = this->whitenResidual(state, res, H, var_order, noise);\n        \n        int dof = res.rows();\n        \n        if (_chi_squared_table.find(dof) == _chi_squared_table.end())\n            for (int i = _chi_squared_table.rbegin()->first+1; i <= dof; ++i)\n            {\n                boost::math::chi_squared chi_squared_dist(i);\n                this->_chi_squared_table[i] = boost::math::quantile(chi_squared_dist, _thres);\n            }\n        \n        if (prob < _chi_squared_table.at(dof))\n            return true;\n        else\n            return false;\n    }\n    \n    bool UpdateBase::testChiSquared(const std::shared_ptr<State> state,\n                                    const Eigen::VectorXd& res,\n                                    const Eigen::MatrixXd& H,\n                                    const std::vector<std::shared_ptr<Type>>& var_order,\n                                    double noise,\n                                    int dof)\n    {\n        double prob = this->whitenResidual(state, res, H, var_order, noise);\n        \n        if (_chi_squared_table.find(dof) == _chi_squared_table.end())\n            for (int i = _chi_squared_table.rbegin()->first+1; i <= dof; ++i)\n            {\n                boost::math::chi_squared chi_squared_dist(i);\n                this->_chi_squared_table[i] = boost::math::quantile(chi_squared_dist, _thres);\n            }\n            \n        if (prob < _chi_squared_table.at(dof))\n            return true;\n        else\n            return false;\n    }\n    \n    bool UpdateBase::testChiSquared(const std::shared_ptr<State> state,\n                                    const Eigen::VectorXd& res,\n                                    const Eigen::MatrixXd& H,\n                                    const std::vector<std::shared_ptr<Type>>& var_order,\n                                    const Eigen::MatrixXd& R,\n                                    int dof)\n    {\n        if (dof <= 0) \n            return false;\n        \n        double prob = this->whitenResidual(state, res, H, var_order, R);\n        \n        if (_chi_squared_table.find(dof) == _chi_squared_table.end())\n            for (int i = _chi_squared_table.rbegin()->first+1; i <= dof; ++i)\n            {\n                boost::math::chi_squared chi_squared_dist(i);\n                this->_chi_squared_table[i] = boost::math::quantile(chi_squared_dist, _thres);\n            }\n            \n        if (prob < _chi_squared_table.at(dof))\n            return true;\n        else\n            return false;\n    }\n}\n"
  },
  {
    "path": "ingvio_estimator/src/Update.h",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#pragma once\n\n#include <memory>\n\n#include <boost/math/distributions/chi_squared.hpp>\n\n#include <vector>\n#include <map>\n#include <Eigen/Dense>\n\nnamespace ingvio\n{\n    class Type;\n    class State;\n    \n    class UpdateBase\n    {\n    public:\n        UpdateBase(const int& max_dof, const double& thres) : _thres(thres)\n        {\n            this->setChiSquaredTable(max_dof, _thres);\n        }\n        \n        UpdateBase(const double& thres) : _thres(thres)\n        {\n            this->setChiSquaredTable(150, _thres);\n        }\n        \n        UpdateBase() : _thres(0.95)\n        {\n            this->setChiSquaredTable(150, _thres);\n        }\n        \n        virtual ~UpdateBase() {}\n        \n        UpdateBase(const UpdateBase&) = delete;\n        \n    protected:\n        \n        double _thres;\n        \n        std::map<int, double> _chi_squared_table;\n        \n        void setChiSquaredTable(const int& max_dof, const double& thres);\n        \n        double whitenResidual(const std::shared_ptr<State> state,\n                              const Eigen::VectorXd& res,\n                              const Eigen::MatrixXd& H,\n                              const std::vector<std::shared_ptr<Type>>& var_order,\n                              double noise);\n        \n        double whitenResidual(const std::shared_ptr<State> state,\n                              const Eigen::VectorXd& res,\n                              const Eigen::MatrixXd& H,\n                              const std::vector<std::shared_ptr<Type>>& var_order,\n                              const Eigen::MatrixXd& R);\n        \n        virtual bool testChiSquared(const std::shared_ptr<State> state,\n                                    const Eigen::VectorXd& res,\n                                    const Eigen::MatrixXd& H,\n                                    const std::vector<std::shared_ptr<Type>>& var_order,\n                                    double noise);\n        \n        virtual bool testChiSquared(const std::shared_ptr<State> state,\n                                    const Eigen::VectorXd& res,\n                                    const Eigen::MatrixXd& H,\n                                    const std::vector<std::shared_ptr<Type>>& var_order,\n                                    double noise,\n                                    int dof);\n        \n        virtual bool testChiSquared(const std::shared_ptr<State> state,\n                                    const Eigen::VectorXd& res,\n                                    const Eigen::MatrixXd& H,\n                                    const std::vector<std::shared_ptr<Type>>& var_order,\n                                    const Eigen::MatrixXd& R,\n                                    int dof);\n    };\n}\n"
  },
  {
    "path": "ingvio_estimator/src/VecState.cpp",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#include \"VecState.h\"\n\nnamespace ingvio\n{\n    void Vec3::update(const Eigen::VectorXd& dx)\n    {\n        assert(dx.rows() >= this->idx() + this->size());\n        _vec += dx.block<3, 1>(this->idx(), 0);\n    }\n    \n    std::shared_ptr<Vec3> Vec3::clone()\n    {\n        auto tmp = std::shared_ptr<Vec3>(new Vec3());\n        \n        tmp->setValue(this->value());\n        tmp->setFej(this->fej());\n        \n        return tmp;\n    }\n    \n    void Scalar::update(const Eigen::VectorXd& dx)\n    {\n        assert(dx.rows() >= this->idx() + this->size());\n        _scalar += dx(this->idx());\n    }\n    \n    void Scalar::setRandom()\n    {\n        Eigen::Vector2d tmp;\n        tmp.setRandom();\n        _scalar = tmp(0);\n    }\n    \n    std::shared_ptr<Scalar> Scalar::clone()\n    {\n        auto tmp = std::shared_ptr<Scalar>(new Scalar());\n        \n        tmp->setValue(this->value());\n        tmp->setFej(this->fej());\n        \n        return tmp;\n    }\n}\n\n"
  },
  {
    "path": "ingvio_estimator/src/VecState.h",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#pragma once\n\n#include <memory>\n#include <Eigen/Core>\n\nnamespace ingvio\n{\n    // Acknowledgements:\n    // The idea of type-based index system is inspired by the realization of OpenVINS\n    // see https://github.com/rpng/open_vins\n    \n    class Type\n    {\n    public:\n        Type(int size) { _size = size; }\n        virtual ~Type(){}\n        \n        virtual void set_cov_idx(int new_cov_idx)\n        {\n            _idx = new_cov_idx;\n        }\n        \n        int idx() const { return _idx; }\n        int size() const {return _size; }\n        \n        virtual void update(const Eigen::VectorXd& dx) = 0;\n        \n        virtual void setIdentity() = 0;\n        virtual void setRandom() = 0;\n        \n    protected:\n        int _idx = -1;\n        int _size = -1;\n    };\n    \n    class Vec3 : public Type\n    {\n    public:\n        Vec3() : Type(3)\n        {\n            _vec.setZero();\n            _vec_fej.setZero();\n        }\n        \n        ~Vec3() {}\n        \n        void update(const Eigen::VectorXd& dx) override;\n        \n        void setIdentity() override\n        {\n            _vec.setZero();\n        }\n        void setRandom() override\n        {\n            _vec.setRandom();\n        }\n        \n        const Eigen::Vector3d& value() const \n        { return _vec; }\n        \n        const Eigen::Vector3d& fej() const \n        { return _vec_fej; }\n        \n        void setValue(const Eigen::Vector3d& other_vec)\n        { _vec = other_vec; }\n        \n        void setFej(const Eigen::Vector3d& other_vec_fej) \n        { _vec_fej = other_vec_fej; }\n        \n        std::shared_ptr<Vec3> clone();\n        \n    protected:\n        Eigen::Vector3d _vec;\n        Eigen::Vector3d _vec_fej;\n    };\n    \n    class Scalar : public Type\n    {\n    public:\n        Scalar() : Type(1)\n        {\n            _scalar = 0.0;\n            _scalar_fej = 0.0;\n        }\n        \n        ~Scalar() {}\n        \n        void update(const Eigen::VectorXd& dx) override;\n        \n        void setIdentity() override\n        {\n            _scalar = 0.0;\n        }\n        \n        void setRandom() override;\n        \n        const double& value() const\n        { return _scalar; }\n        \n        const double& fej() const \n        { return _scalar_fej; }\n        \n        void setValue(const double& other_scalar) \n        { _scalar = other_scalar; }\n        \n        void setFej(const double& other_scalar_fej) \n        { _scalar_fej = other_scalar_fej; }\n        \n        std::shared_ptr<Scalar> clone();\n        \n    protected:\n        double _scalar;\n        double _scalar_fej;\n    };\n}\n"
  },
  {
    "path": "ingvio_estimator/test/GenerateNoise.cpp",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#include \"GenerateNoise.h\"\n\n#include <cmath>\n\nnamespace ingvio_test\n{\n    double generateGaussRandom(const double& mean, const double& std)\n    {\n        double z1;\n        do\n        {\n            int x1, x2;\n            x1 = rand();\n            x2 = rand();\n            double y1 = static_cast<double>(x1 % 100)/100.0;\n            double y2 = static_cast<double>(x2 % 100)/100.0;\n            z1 = std::sqrt(-2.0*std::log(y1))*std::sin(2.0*M_PI*y2);\n        }\n        while (std::isnan(mean+z1*std) || std::isinf(mean+z1*std));\n        return mean+z1*std;\n    }\n    \n    bool isSPD(const Eigen::MatrixXd& cov)\n    {\n        assert(cov.rows() == cov.cols());\n        \n        Eigen::VectorXd diags = cov.diagonal();\n        \n        for (int i = 0; i < diags.rows(); ++i)\n            if (diags(i, 0) < 0.0)\n                return false;\n            \n        return true;\n    }\n}\n"
  },
  {
    "path": "ingvio_estimator/test/GenerateNoise.h",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#pragma once\n\n#include <Eigen/Core>\n\nnamespace ingvio_test\n{\n    extern double generateGaussRandom(const double& mean, const double& std);\n    \n    extern bool isSPD(const Eigen::MatrixXd& cov);\n}\n"
  },
  {
    "path": "ingvio_estimator/test/TestMapServer.cpp",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#include <gtest/gtest.h>\n\n#include <ros/ros.h>\n#include <std_msgs/Header.h>\n\n#include \"MapServer.h\"\n#include \"MapServerManager.h\"\n\n#include \"State.h\"\n#include \"StateManager.h\"\n\n#include \"ImuPropagator.h\"\n\n#include <ros/ros.h>\n#include <std_msgs/Header.h>\n\n#include \"GenerateNoise.h\"\n#include \"Triangulator.h\"\n\nusing namespace ingvio;\n\nnamespace ingvio_test\n{\n    class TestMapServer : public virtual ::testing::Test\n    {\n    public:\n        TestMapServer ()\n        {\n            T_cl2cr.linear() = Eigen::Matrix3d::Identity();\n            T_cl2cr.translation() = Eigen::Vector3d(0.001, -0.12, 0.003);\n            \n            filter_params.readParams(\"/home/lcw/VIO/ws_ingvio/src/config/sportsfield/ingvio_stereo.yaml\");\n            filter_params._init_imu_buffer_sp = -1;\n            \n            state = std::make_shared<State>(filter_params);\n            state->_state_params._T_cl2cr = T_cl2cr;\n            \n            imu_propa = std::make_shared<ImuPropagator>(filter_params);\n            \n            state->initStateAndCov(0.0, Eigen::Quaterniond::Identity());\n            \n            map_server = std::make_shared<MapServer>();\n        }\n        \n        virtual ~TestMapServer() = default;\n    protected:\n        \n        Eigen::Isometry3d T_cl2cr;\n        \n        IngvioParams filter_params;\n        \n        std::shared_ptr<State> state;\n        \n        std::shared_ptr<ImuPropagator> imu_propa;\n        \n        std::shared_ptr<MapServer> map_server;\n        \n        feature_tracker::MonoFrameConstPtr generateRandomMonoFrame(const std::vector<int>& ids, double timestamp)\n        {\n            feature_tracker::MonoFramePtr mono_frame(new feature_tracker::MonoFrame());\n            \n            mono_frame->header.stamp = ros::Time(timestamp);\n            \n            for (int i = 0; i < ids.size(); ++i)\n            {\n                feature_tracker::MonoMeas::Ptr mono_meas(new feature_tracker::MonoMeas());\n                \n                mono_meas->id = ids[i];\n                \n                Eigen::Vector2d vec = Eigen::Vector2d::Random();\n                mono_meas->u0 = vec.x();\n                mono_meas->v0 = vec.y();\n                \n                mono_frame->mono_features.push_back(*mono_meas);\n            }\n            \n            return mono_frame;\n        }\n        \n        feature_tracker::MonoMeas::Ptr calcMonoMeas(int id, const Eigen::Vector3d& pf, const std::shared_ptr<SE3> pose)\n        {\n            feature_tracker::MonoMeas::Ptr mono_meas(new feature_tracker::MonoMeas());\n            \n            Eigen::Vector3d body = pose->copyValueAsIso().inverse()*pf;\n            \n            mono_meas->id = id;\n            mono_meas->u0 = body.x()/body.z() + generateGaussRandom(0.0, 0.02);\n            mono_meas->v0 = body.y()/body.z() + generateGaussRandom(0.0, 0.02);\n            \n            return mono_meas;\n        }\n        \n        feature_tracker::MonoFrameConstPtr generateMonoFrame(double timestamp, const std::vector<int>& ids, const std::vector<Eigen::Vector3d>& pfs, const std::shared_ptr<SE3> pose)\n        {\n            feature_tracker::MonoFramePtr mono_frame(new feature_tracker::MonoFrame());\n            \n            mono_frame->header.stamp = ros::Time(timestamp);\n            \n            assert(ids.size() == pfs.size());\n            \n            for (int i = 0; i < ids.size(); ++i)\n                mono_frame->mono_features.push_back(*(calcMonoMeas(ids[i], pfs[i], pose)));\n            \n            return mono_frame;\n        }\n        \n        feature_tracker::StereoFrameConstPtr generateRandomStereoFrame(const std::vector<int>& ids, double timestamp)\n        {\n            feature_tracker::StereoFramePtr stereo_frame(new feature_tracker::StereoFrame());\n            \n            stereo_frame->header.stamp = ros::Time(timestamp);\n            \n            for (int i = 0; i < ids.size(); ++i)\n            {\n                feature_tracker::StereoMeas::Ptr stereo_meas(new feature_tracker::StereoMeas());\n                \n                stereo_meas->id = ids[i];\n                \n                Eigen::Vector4d vec = Eigen::Vector4d::Random();\n                stereo_meas->u0 = vec.x();\n                stereo_meas->v0 = vec.y();\n                stereo_meas->u1 = vec.z();\n                stereo_meas->v1 = vec.w();\n                \n                stereo_frame->stereo_features.push_back(*stereo_meas);\n            }\n            \n            return stereo_frame;\n        }\n        \n        feature_tracker::StereoMeas::Ptr calcStereoMeas(int id, const Eigen::Vector3d& pf, const std::shared_ptr<SE3> pose, const Eigen::Isometry3d& T_cl2cr)\n        {\n            feature_tracker::StereoMeas::Ptr stereo_meas(new feature_tracker::StereoMeas());\n            \n            Eigen::Vector3d body_left = pose->copyValueAsIso().inverse()*pf;\n            Eigen::Vector3d body_right = T_cl2cr*pose->copyValueAsIso().inverse()*pf;\n            \n            stereo_meas->id = id;\n            \n            stereo_meas->u0 = body_left.x()/body_left.z() + generateGaussRandom(0.0, 0.02);\n            stereo_meas->v0 = body_left.y()/body_left.z() + generateGaussRandom(0.0, 0.02);\n            \n            stereo_meas->u1 = body_right.x()/body_right.z() + generateGaussRandom(0.0, 0.02);\n            stereo_meas->v1 = body_right.y()/body_right.z() + generateGaussRandom(0.0, 0.02);\n            \n            return stereo_meas;\n        }\n        \n        feature_tracker::StereoFrameConstPtr generateStereoFrame(double timestamp, const std::vector<int>& ids, const std::vector<Eigen::Vector3d>& pfs, const std::shared_ptr<SE3> pose, const Eigen::Isometry3d& T_cl2cr)\n        {\n            feature_tracker::StereoFramePtr stereo_frame(new feature_tracker::StereoFrame());\n            \n            stereo_frame->header.stamp = ros::Time(timestamp);\n            \n            assert(ids.size() == pfs.size());\n            \n            for (int i = 0; i < ids.size(); ++i)\n                stereo_frame->stereo_features.push_back(*(calcStereoMeas(ids[i], pfs[i], pose, T_cl2cr)));\n            \n            return stereo_frame;\n        }\n    };\n    \n    TEST_F(TestMapServer, collectFeatureAndMarg)\n    {\n        std::vector<int> id1(4);\n        for (int i = 0; i < id1.size(); ++i)\n            id1[i] = i+1;\n        \n        std::vector<int> id2(4);\n        for (int i = 0; i < id2.size(); ++i)\n            id2[i] = i+2;\n\n        Eigen::Isometry3d T;\n        Eigen::Vector3d rot = Eigen::Vector3d::Random();\n        Eigen::Vector3d trans = Eigen::Vector3d::Random();\n        \n        T.linear() = Eigen::AngleAxisd(rot.norm(), rot.normalized()).toRotationMatrix();\n        T.translation() = trans;\n        \n        imu_propa->propagateToExpectedPoseAndAugment(state, 2.0, T);\n        \n        MapServerManager::collectMonoMeas(map_server, state, generateRandomMonoFrame(id1, 2.0));\n        \n        ASSERT_EQ(map_server->size(), 4);\n        \n        for (int i = 1; i < 5; ++i)\n        {\n            ASSERT_EQ(map_server->at(i)->getId(), i);\n            ASSERT_EQ(map_server->at(i)->getFeatureType(), FeatureInfo::FeatureType::MSCKF);\n            ASSERT_EQ(map_server->at(i)->numOfMonoFrames(), 1);\n            ASSERT_TRUE(map_server->at(i)->hasMonoObsAt(2.0));\n        }\n        \n        imu_propa->propagateToExpectedPoseAndAugment(state, 4.0, T);\n        \n        MapServerManager::collectMonoMeas(map_server, state, generateRandomMonoFrame(id2, 4.0));\n        \n        ASSERT_EQ(map_server->size(), 5);\n        \n        for (int i = 1; i < 6; ++i)\n        {\n            ASSERT_EQ(map_server->at(i)->getId(), i);\n            ASSERT_EQ(map_server->at(i)->getFeatureType(), FeatureInfo::FeatureType::MSCKF);\n            if (i == 1 || i == 5)\n                ASSERT_EQ(map_server->at(i)->numOfMonoFrames(), 1);\n            else\n                ASSERT_EQ(map_server->at(i)->numOfMonoFrames(), 2);\n                \n            if (i > 1)\n                ASSERT_TRUE(map_server->at(i)->hasMonoObsAt(4.0));\n            \n            ASSERT_TRUE(!map_server->at(i)->isToMarg());\n        }\n        \n        MapServerManager::markMargMonoFeatures(map_server, state);\n        \n        ASSERT_EQ(map_server->size(), 5);\n        \n        for (int i = 1; i < 6; ++i)\n        {\n            if (i == 1)\n                ASSERT_TRUE(map_server->at(i)->isToMarg());\n            else\n                ASSERT_TRUE(!map_server->at(i)->isToMarg());\n        }\n        \n        state.reset(new State(filter_params));\n        \n        state->initStateAndCov(0.0, Eigen::Quaterniond::Identity());\n        \n        imu_propa.reset(new ImuPropagator(filter_params));\n        \n        map_server.reset(new MapServer());\n        \n        imu_propa->propagateToExpectedPoseAndAugment(state, 2.0, T);\n        \n        MapServerManager::collectStereoMeas(map_server, state, generateRandomStereoFrame(id1, 2.0));\n        \n        ASSERT_EQ(map_server->size(), 4);\n        \n        for (int i = 1; i < 5; ++i)\n        {\n            ASSERT_EQ(map_server->at(i)->getId(), i);\n            ASSERT_EQ(map_server->at(i)->getFeatureType(), FeatureInfo::FeatureType::MSCKF);\n            ASSERT_EQ(map_server->at(i)->numOfStereoFrames(), 1);\n            ASSERT_TRUE(map_server->at(i)->hasStereoObsAt(2.0));\n        }\n        \n        imu_propa->propagateToExpectedPoseAndAugment(state, 4.0, T);\n        \n        MapServerManager::collectStereoMeas(map_server, state, generateRandomStereoFrame(id2, 4.0));\n        \n        ASSERT_EQ(map_server->size(), 5);\n        \n        for (int i = 1; i < 6; ++i)\n        {\n            ASSERT_EQ(map_server->at(i)->getId(), i);\n            ASSERT_EQ(map_server->at(i)->getFeatureType(), FeatureInfo::FeatureType::MSCKF);\n            if (i == 1 || i == 5)\n                ASSERT_EQ(map_server->at(i)->numOfStereoFrames(), 1);\n            else\n                ASSERT_EQ(map_server->at(i)->numOfStereoFrames(), 2);\n            \n            if (i > 1)\n                ASSERT_TRUE(map_server->at(i)->hasStereoObsAt(4.0));\n            \n            ASSERT_TRUE(!map_server->at(i)->isToMarg());\n        }\n        \n        MapServerManager::markMargStereoFeatures(map_server, state);\n        \n        ASSERT_EQ(map_server->size(), 5);\n        \n        for (int i = 1; i < 6; ++i)\n        {\n            if (i == 1)\n                ASSERT_TRUE(map_server->at(i)->isToMarg());\n            else\n                ASSERT_TRUE(!map_server->at(i)->isToMarg());\n        }\n        \n        for (int i = 1; i < 6; ++i)\n            if (i < 5)\n                ASSERT_TRUE(map_server->at(i)->anchor() == static_cast<const std::shared_ptr<SE3>>(state->_sw_camleft_poses.at(2.0)));\n            else\n                ASSERT_TRUE((map_server->at(i)->anchor() == static_cast<const std::shared_ptr<SE3>>(state->_sw_camleft_poses.at(4.0))));\n    }\n    \n    TEST_F(TestMapServer, featureInfoTriMono)\n    {\n        std::vector<int> ids1(2);\n        ids1[0] = 1;\n        ids1[1] = 2;\n        \n        std::vector<Eigen::Vector3d> pfs1(2);\n        pfs1[0] = Eigen::Vector3d(1.0, 1.5, 2.0);\n        pfs1[1] = Eigen::Vector3d(1.0, 2.0, 3.0);\n        \n        std::vector<int> ids2(2);\n        ids2[0] = 2;\n        ids2[1] = 3;\n        \n        std::vector<Eigen::Vector3d> pfs2(2);\n        pfs2[0] = pfs1[1];\n        pfs2[1] = Eigen::Vector3d(1.5, 2.0, 3.0);\n        \n        std::vector<double> time(9);\n        for (int i = 0; i < time.size(); ++i)\n            time[i] = 0.5*(i+1);\n        \n        std::vector<Eigen::Isometry3d> T(9);\n        for (int i = 0; i < T.size(); ++i)\n        {\n            T[i].linear() = Eigen::AngleAxisd(generateGaussRandom(0.0, 0.1), Eigen::Vector3d(0, 0, 1)).toRotationMatrix();\n            T[i].translation() = Eigen::Vector3d(2*i-5.0, 2*i-6.0, 0.0);\n        }\n        \n        std::vector<std::shared_ptr<SE3>> poses(9);\n        for (int i = 0; i < 9; ++i)\n        {\n            poses[i] = std::make_shared<SE3>();\n            poses[i]->setValueByIso(T[i]);\n        }\n        \n        imu_propa->propagateToExpectedPoseAndAugment(state, time[0], T[0]);\n        MapServerManager::collectMonoMeas(map_server, state, generateMonoFrame(time[0], ids1, pfs1, poses[0]));\n        \n        imu_propa->propagateToExpectedPoseAndAugment(state, time[1], T[1]);\n        MapServerManager::collectMonoMeas(map_server, state, generateMonoFrame(time[1], ids1, pfs1, poses[1]));\n        \n        imu_propa->propagateToExpectedPoseAndAugment(state, time[2], T[2]);\n        MapServerManager::collectMonoMeas(map_server, state, generateMonoFrame(time[2], ids1, pfs1, poses[2]));\n        \n        imu_propa->propagateToExpectedPoseAndAugment(state, time[3], T[3]);\n        MapServerManager::collectMonoMeas(map_server, state, generateMonoFrame(time[3], ids1, pfs1, poses[3]));\n        \n        imu_propa->propagateToExpectedPoseAndAugment(state, time[4], T[4]);\n        MapServerManager::collectMonoMeas(map_server, state, generateMonoFrame(time[4], ids2, pfs2, poses[4]));\n        \n        imu_propa->propagateToExpectedPoseAndAugment(state, time[5], T[5]);\n        MapServerManager::collectMonoMeas(map_server, state, generateMonoFrame(time[5], ids2, pfs2, poses[5]));\n        \n        std::shared_ptr<Triangulator> tri(new Triangulator());\n        \n        Eigen::Vector3d result;\n        bool flag;\n        flag = FeatureInfoManager::triangulateFeatureInfoMono(map_server->at(1), tri, state);\n        \n        std::cout << \"pf1 ref = \" << pfs1[0].transpose() << std::endl;\n        std::cout << \"pf1 mono estimated = \" << map_server->at(1)->landmark()->valuePosXyz().transpose() << std::endl;\n        \n        ASSERT_TRUE(map_server->at(1)->isTri() == flag);\n        \n        flag = FeatureInfoManager::triangulateFeatureInfoMono(map_server->at(2), tri, state);\n        std::cout << \"pf2 ref = \" << pfs1[1].transpose() << std::endl;\n        std::cout << \"pf2 mono estimated = \" << map_server->at(2)->landmark()->valuePosXyz().transpose() << std::endl;\n        \n        ASSERT_TRUE(map_server->at(2)->isTri() == flag);\n        \n        imu_propa->propagateToExpectedPoseAndAugment(state, time[6], T[6]);\n        MapServerManager::collectMonoMeas(map_server, state, generateMonoFrame(time[6], ids2, pfs2, poses[6]));\n        \n        imu_propa->propagateToExpectedPoseAndAugment(state, time[7], T[7]);\n        MapServerManager::collectMonoMeas(map_server, state, generateMonoFrame(time[7], ids2, pfs2, poses[7]));\n        \n        imu_propa->propagateToExpectedPoseAndAugment(state, time[8], T[8]);\n        MapServerManager::collectMonoMeas(map_server, state, generateMonoFrame(time[8], ids2, pfs2, poses[8]));\n        \n        flag = FeatureInfoManager::triangulateFeatureInfoMono(map_server->at(3), tri, state);\n        std::cout << \"pf3 ref = \" << pfs2[1].transpose() << std::endl;\n        std::cout << \"pf3 mono estimated = \" << map_server->at(3)->landmark()->valuePosXyz().transpose() << std::endl;\n        \n        ASSERT_TRUE(map_server->at(3)->isTri() == flag);\n    }\n    \n    TEST_F(TestMapServer, featureInfoTriStereo)\n    {\n        std::vector<int> ids1(2);\n        ids1[0] = 1;\n        ids1[1] = 2;\n        \n        std::vector<Eigen::Vector3d> pfs1(2);\n        pfs1[0] = Eigen::Vector3d(1.0, 1.5, 2.0);\n        pfs1[1] = Eigen::Vector3d(1.0, 2.0, 3.0);\n        \n        std::vector<int> ids2(2);\n        ids2[0] = 2;\n        ids2[1] = 3;\n        \n        std::vector<Eigen::Vector3d> pfs2(2);\n        pfs2[0] = pfs1[1];\n        pfs2[1] = Eigen::Vector3d(1.5, 2.0, 3.0);\n        \n        std::vector<double> time(9);\n        for (int i = 0; i < time.size(); ++i)\n            time[i] = 0.5*(i+1);\n        \n        std::vector<Eigen::Isometry3d> T(9);\n        for (int i = 0; i < T.size(); ++i)\n        {\n            T[i].linear() = Eigen::AngleAxisd(generateGaussRandom(0.0, 0.1), Eigen::Vector3d(0, 0, 1)).toRotationMatrix();\n            T[i].translation() = Eigen::Vector3d(2*i-5.0, 2*i-6.0, 0.0);\n        }\n        \n        std::vector<std::shared_ptr<SE3>> poses(9);\n        for (int i = 0; i < 9; ++i)\n        {\n            poses[i] = std::make_shared<SE3>();\n            poses[i]->setValueByIso(T[i]);\n        }\n        \n        imu_propa->propagateToExpectedPoseAndAugment(state, time[0], T[0]);\n        MapServerManager::collectStereoMeas(map_server, state, generateStereoFrame(time[0], ids1, pfs1, poses[0], T_cl2cr));\n        \n        imu_propa->propagateToExpectedPoseAndAugment(state, time[1], T[1]);\n        MapServerManager::collectStereoMeas(map_server, state, generateStereoFrame(time[1], ids1, pfs1, poses[1], T_cl2cr));\n        \n        imu_propa->propagateToExpectedPoseAndAugment(state, time[2], T[2]);\n        MapServerManager::collectStereoMeas(map_server, state, generateStereoFrame(time[2], ids1, pfs1, poses[2], T_cl2cr));\n        \n        imu_propa->propagateToExpectedPoseAndAugment(state, time[3], T[3]);\n        MapServerManager::collectStereoMeas(map_server, state, generateStereoFrame(time[3], ids1, pfs1, poses[3], T_cl2cr));\n        \n        imu_propa->propagateToExpectedPoseAndAugment(state, time[4], T[4]);\n        MapServerManager::collectStereoMeas(map_server, state, generateStereoFrame(time[4], ids2, pfs2, poses[4], T_cl2cr));\n        \n        imu_propa->propagateToExpectedPoseAndAugment(state, time[5], T[5]);\n        MapServerManager::collectStereoMeas(map_server, state, generateStereoFrame(time[5], ids2, pfs2, poses[5], T_cl2cr));\n        \n        std::shared_ptr<Triangulator> tri(new Triangulator());\n        \n        Eigen::Vector3d result;\n        bool flag;\n        flag = FeatureInfoManager::triangulateFeatureInfoStereo(map_server->at(1), tri, state);\n        \n        std::cout << \"pf1 ref = \" << pfs1[0].transpose() << std::endl;\n        std::cout << \"pf1 stereo estimated = \" << map_server->at(1)->landmark()->valuePosXyz().transpose() << std::endl;\n        \n        ASSERT_TRUE(map_server->at(1)->isTri() == flag);\n        \n        flag = FeatureInfoManager::triangulateFeatureInfoStereo(map_server->at(2), tri, state);\n        std::cout << \"pf2 ref = \" << pfs1[1].transpose() << std::endl;\n        std::cout << \"pf2 stereo estimated = \" << map_server->at(2)->landmark()->valuePosXyz().transpose() << std::endl;\n        \n        ASSERT_TRUE(map_server->at(2)->isTri() == flag);\n        \n        imu_propa->propagateToExpectedPoseAndAugment(state, time[6], T[6]);\n        MapServerManager::collectStereoMeas(map_server, state, generateStereoFrame(time[6], ids2, pfs2, poses[6], T_cl2cr));\n        \n        imu_propa->propagateToExpectedPoseAndAugment(state, time[7], T[7]);\n        MapServerManager::collectStereoMeas(map_server, state, generateStereoFrame(time[7], ids2, pfs2, poses[7], T_cl2cr));\n        \n        imu_propa->propagateToExpectedPoseAndAugment(state, time[8], T[8]);\n        MapServerManager::collectStereoMeas(map_server, state, generateStereoFrame(time[8], ids2, pfs2, poses[8], T_cl2cr));\n        \n        flag = FeatureInfoManager::triangulateFeatureInfoStereo(map_server->at(3), tri, state);\n        std::cout << \"pf3 ref = \" << pfs2[1].transpose() << std::endl;\n        std::cout << \"pf3 stereo estimated = \" << map_server->at(3)->landmark()->valuePosXyz().transpose() << std::endl;\n        \n        ASSERT_TRUE(map_server->at(3)->isTri() == flag);\n    }\n    \n    TEST_F(TestMapServer, changeAnchor)\n    {\n        for (int i = 0; i < 500; ++i)\n        {\n            ImuCtrl imu_ctrl;\n            \n            imu_ctrl._gyro_raw = Eigen::Vector3d(0.0, 0.0, 0.0);\n            imu_ctrl._accel_raw = Eigen::Vector3d(0.0, 0.0, 9.8);\n            imu_ctrl._timestamp = 0.01*i;\n            \n            imu_propa->storeImu(imu_ctrl);\n        }\n        \n        std::vector<int> ids(1);\n        ids[0] = 1;\n        std::vector<Eigen::Vector3d> pfs(1);\n        pfs[0] = Eigen::Vector3d(1.0, 1.0, 6.0);\n        \n        imu_propa->propagateAugmentAtEnd(state, 1.8);\n        MapServerManager::collectMonoMeas(map_server, state, generateMonoFrame(1.8, ids, pfs, state->_sw_camleft_poses.at(1.8)));\n        \n        StateManager::addAnchoredLandmarkInState(state, map_server->at(1)->landmark(), map_server->at(1)->getId(), 2.21*Eigen::Matrix3d::Identity());\n        \n        imu_propa->propagateAugmentAtEnd(state, 3.6);\n        MapServerManager::collectMonoMeas(map_server, state, generateMonoFrame(3.6, ids, pfs, state->_sw_camleft_poses.at(3.6)));\n        \n        map_server->at(1)->_ftype = FeatureInfo::FeatureType::SLAM;\n        map_server->at(1)->_landmark->setValuePosXyz(pfs[0]);\n        \n        ASSERT_EQ(map_server->at(1)->getId(), 1);\n        ASSERT_EQ(map_server->at(1)->isTri(), false);\n        ASSERT_EQ(map_server->at(1)->isToMarg(), false);\n        \n        ASSERT_EQ(map_server->at(1)->_landmark->getAnchoredPose(), state->_sw_camleft_poses.at(1.8));\n        \n        ASSERT_EQ(map_server->at(1)->numOfMonoFrames(), 2);\n        ASSERT_EQ(state->_anchored_landmarks.at(1)->getAnchoredPose(), state->_sw_camleft_poses.at(1.8));\n        \n        ASSERT_NEAR((state->_anchored_landmarks.at(1)->valuePosXyz()-pfs[0]).norm(), 0.0, 1e-06);\n        ASSERT_NEAR((map_server->at(1)->_landmark->valuePosXyz()-pfs[0]).norm(), 0.0, 1e-06);\n                \n        ASSERT_TRUE(isSPD(StateManager::getFullCov(state)));\n        \n        Eigen::MatrixXd cov_orig = StateManager::getFullCov(state);\n        Eigen::MatrixXd cov = StateManager::getFullCov(state);\n        Eigen::MatrixXd H = Eigen::MatrixXd::Zero(3, 36);\n        \n        // std::cout << \"cov ref = \" << cov << std::endl;\n        \n        H.block<3, 3>(0, 27) = Eigen::Matrix3d::Identity();\n        H.block<3, 3>(0, 21) = -skew(pfs[0]);\n        H.block<3, 3>(0, 30) = skew(pfs[0]);\n        \n        FeatureInfoManager::changeAnchoredPose(map_server->at(1), state, 3.6);\n        \n        ASSERT_EQ(map_server->at(1)->_landmark->getAnchoredPose(), state->_sw_camleft_poses.at(3.6));\n        ASSERT_EQ(state->_anchored_landmarks.at(1)->getAnchoredPose(), state->_sw_camleft_poses.at(3.6));\n        \n        ASSERT_TRUE(isSPD(StateManager::getFullCov(state)));\n        \n        cov.block<36, 3>(0, 27) = cov_orig*H.transpose();\n        cov.block<3, 36>(27, 0) = H*cov_orig;\n        cov.block<3, 3>(27, 27) = H*cov_orig*H.transpose();\n        \n        // std::cout << \"cov ref = \" << cov << std::endl;\n        // std::cout << \"cov diff = \" << cov - StateManager::getFullCov(state) << std::endl;\n        \n        ASSERT_NEAR((cov-StateManager::getFullCov(state)).norm(), 0.0, 1e-10);\n    }\n}\n\nint main(int argc, char** argv)\n{\n    testing::InitGoogleTest(&argc, argv);\n    return RUN_ALL_TESTS();\n}\n"
  },
  {
    "path": "ingvio_estimator/test/TestPropagator.cpp",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#include <gtest/gtest.h>\n\n#include \"IngvioParams.h\"\n#include \"ImuPropagator.h\"\n\n#include \"State.h\"\n#include \"StateManager.h\"\n\n#include \"TicToc.h\"\n\nusing namespace ingvio;\n\nnamespace ingvio_test\n{\n    class TestPropagator : public virtual ::testing::Test\n    {\n    public:\n        TestPropagator() : _N(300)\n        {\n            sf = Eigen::Vector3d::Random().normalized();\n            sf *= 9.75;\n        }\n        \n        virtual ~TestPropagator() {}\n    protected:\n        int _N;\n        \n        IngvioParams filter_params;\n        \n        Eigen::Vector3d sf;\n\n    };\n    \n    TEST_F(TestPropagator, initGravity)\n    {\n        filter_params.readParams(\"/home/lcw/VIO/ws_ingvio/src/config/sportsfield/ingvio_stereo.yaml\");\n        filter_params.printParams();\n        \n        ImuPropagator ip2;\n        \n        ImuPropagator ip1(filter_params);\n        \n        for (int i = 0; i < _N; ++i)\n        {\n            ImuCtrl imu_ctrl;\n            imu_ctrl._accel_raw = sf;\n            imu_ctrl._gyro_raw.setZero();\n            \n            imu_ctrl._timestamp = 0.01*i;\n            \n            ip1.storeImu(imu_ctrl);\n            ip2.storeImu(imu_ctrl);\n        }\n        \n        auto isMatNear = [](const Eigen::MatrixXd& mat1, const Eigen::MatrixXd& mat2)\n        {\n            if ((mat1-mat2).norm() < 1e-08)\n                return true;\n            else\n                return false;\n        };\n        \n        auto isQuatNear = [](const Eigen::Quaterniond& quat1, const Eigen::Quaterniond& quat2)\n        {\n            if ((quat1.toRotationMatrix()-quat2.toRotationMatrix()).norm() < 1e-08)\n                return true;\n            else\n                return false;\n        };\n        \n        ASSERT_TRUE(isMatNear(Eigen::Vector3d(0, 0, -sf.norm()), ip1.getGravity()));\n        ASSERT_TRUE(isMatNear(Eigen::Vector3d(0, 0, -9.8), ip2.getGravity()));\n        \n        Eigen::Quaterniond init_quat1, init_quat2;\n        ASSERT_TRUE(ip1.getInitQuat(init_quat1));\n        ASSERT_TRUE(ip2.getInitQuat(init_quat2));\n        \n        ASSERT_TRUE(isQuatNear(init_quat2, Eigen::Quaterniond::Identity()));\n        \n        Eigen::Quaterniond quat_ref = Eigen::Quaterniond::FromTwoVectors(-sf, Eigen::Vector3d(0, 0, -sf.norm()));\n        \n        ASSERT_TRUE(isQuatNear(init_quat1, quat_ref));\n        \n        Eigen::Quaterniond tmp_quat1, tmp_quat2;\n        for (int i = 1; i < 400; i += 20)\n        {\n            ASSERT_TRUE(ip1.getAvgQuat(tmp_quat1, i));\n            ASSERT_TRUE(ip2.getAvgQuat(tmp_quat2, i));\n            ASSERT_TRUE(isQuatNear(quat_ref, tmp_quat1));\n            ASSERT_TRUE(isQuatNear(quat_ref, tmp_quat2));\n        }\n    }\n    \n    TEST_F(TestPropagator, oneStepProp)\n    {\n        filter_params.readParams(\"/home/lcw/VIO/ws_ingvio/src/config/sportsfield/ingvio_stereo.yaml\");\n        \n        filter_params._init_imu_buffer_sp = -1;\n        \n        ImuPropagator ip(filter_params);\n        \n        ImuCtrl imu_ctrl;\n        imu_ctrl.setRandom();\n        \n        std::shared_ptr<State> state1, state2;\n        \n        auto reset = [](std::shared_ptr<State>& state1, std::shared_ptr<State>& state2, const IngvioParams& filter_params)\n        {\n            state1.reset(new State(filter_params));\n            state2.reset(new State(filter_params));\n            \n            state1->_timestamp = 1.0;\n            state2->_timestamp = 1.0;\n            \n            state1->_extended_pose->setRandom();\n            state2->_extended_pose = state1->_extended_pose->clone();\n            \n            state1->_bg->setRandom();\n            state2->_bg = state1->_bg->clone();\n            \n            state1->_ba->setRandom();\n            state2->_ba = state1->_ba->clone();\n            \n            StateManager::addGNSSVariable(state1, State::GNSSType::GPS, 5000, 9.0);\n            StateManager::addGNSSVariable(state1, State::GNSSType::FS, 20, 2.0);\n            \n            StateManager::addGNSSVariable(state2, State::GNSSType::GPS, 5000, 9.0);\n            StateManager::addGNSSVariable(state2, State::GNSSType::FS, 20, 2.0);\n        };\n        \n        reset(state1, state2, filter_params);\n        ASSERT_NEAR(distance(state1, state2), 0.0, 1e-12);\n        \n        double err1 = INFINITY;\n        double err2 = INFINITY;\n        \n        Eigen::Matrix<double, 15, 15> Phi1, Phi2;\n        Eigen::Matrix<double, 15, 12> G1, G2;\n        \n        double t_analytic = 0.0;\n        double t_numerical = 0.0;\n        \n        for (int i = 0; i < 5; ++i)\n        {\n            reset(state1, state2, filter_params);\n            \n            double dt = std::pow(10.0, -i);\n            \n            TicToc analytic;\n            ip.stateAndCovTransition(state1, imu_ctrl, dt, Phi1, G1, true);\n            t_analytic += analytic.toc();\n            \n            TicToc numerical;\n            ip.stateAndCovTransition(state2, imu_ctrl, dt, Phi2, G2, false);\n            t_numerical += numerical.toc();\n            \n            double err_state = distance(state1, state2);\n            double err_Phi = (Phi1-Phi2).norm();\n            \n            ASSERT_TRUE(err_state < err1);\n            ASSERT_TRUE(err_Phi < err2);\n            \n            err1 = err_state;\n            err2 = err_Phi;\n        }\n        \n        std::cout << \"Analytic one step prop: \" << t_analytic << \" (ms)\" << std::endl;\n        std::cout << \"Numerical one step prop: \" << t_numerical << \" (ms)\" << std::endl;\n    }\n    \n    TEST_F(TestPropagator, propaUntil)\n    {\n        filter_params.readParams(\"/home/lcw/VIO/ws_ingvio/src/config/sportsfield/ingvio_stereo.yaml\");\n        filter_params._init_imu_buffer_sp = -1;\n        \n        std::shared_ptr<State> state = std::make_shared<State>(filter_params);\n        \n        ImuPropagator ip(filter_params);\n        \n        ASSERT_TRUE(ip.isInit());\n        \n        state->initStateAndCov(0.0, Eigen::Quaterniond::Identity());\n        \n        for (int i = -20; i <= 100; ++i)\n        {\n            ImuCtrl imu_ctrl;\n            imu_ctrl.setRandom();\n            imu_ctrl._timestamp = 0.1*i;\n            \n            ip.storeImu(imu_ctrl);\n        }\n        \n        ip.propagateUntil(state, 1.0);\n        ASSERT_EQ(state->_timestamp, 1.0);\n        \n        ip.propagateUntil(state, 8.35);\n        ASSERT_EQ(state->_timestamp, 8.35);\n        \n        ip.propagateUntil(state, 7.1);\n        ASSERT_EQ(state->_timestamp, 8.35);\n        \n        ip.propagateUntil(state, 9.5);\n        ASSERT_EQ(state->_timestamp, 9.5);\n        \n        ip.propagateUntil(state, 10.5);\n        ASSERT_EQ(state->_timestamp, 10.5);\n        \n        ip.propagateUntil(state, 11.0);\n        ASSERT_EQ(state->_timestamp, 10.5);\n    }\n    \n    TEST_F(TestPropagator, propagateAugment)\n    {\n        filter_params.readParams(\"/home/lcw/VIO/ws_ingvio/src/config/sportsfield/ingvio_stereo.yaml\");\n        filter_params._init_imu_buffer_sp = -1;\n        \n        std::shared_ptr<State> state = std::make_shared<State>(filter_params);\n        \n        ImuPropagator ip(filter_params);\n        \n        ASSERT_TRUE(ip.isInit());\n        \n        state->initStateAndCov(0.0, Eigen::Quaterniond::Identity());\n        \n        for (int i = -20; i <= 100; ++i)\n        {\n            ImuCtrl imu_ctrl;\n            imu_ctrl.setRandom();\n            imu_ctrl._timestamp = 0.1*i;\n            \n            ip.storeImu(imu_ctrl);\n        }\n        \n        for (int i = 0; i < 15; ++i)\n            ip.propagateAugmentAtEnd(state, (i+1)*0.5);\n        \n        ASSERT_EQ(state->_timestamp, 7.5);\n        ASSERT_EQ(state->curr_cov_size(), 21 + 6*15);\n    }\n}\n\nint main(int argc, char** argv)\n{\n    testing::InitGoogleTest(&argc, argv);\n    return RUN_ALL_TESTS();\n}\n"
  },
  {
    "path": "ingvio_estimator/test/TestStateManager.cpp",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#include <gtest/gtest.h>\n#include <Eigen/Geometry>\n\n#include \"State.h\"\n#include \"StateManager.h\"\n\nusing namespace ingvio;\n\nnamespace ingvio_test\n{\n    TEST(testState, BasicFuncs)\n    {\n        for (int i = 0; i < 20; ++i)\n        {\n            Eigen::Vector3d vec;\n            vec.setRandom();\n            \n            ASSERT_TRUE(skew(vec) == -skew(vec).transpose());\n            ASSERT_TRUE(vee(skew(vec)) == vec);\n            \n            Eigen::Matrix3d rot_mat = Eigen::AngleAxisd(vec.norm(), vec.normalized()).toRotationMatrix();\n            \n            ASSERT_NEAR((GammaFunc(vec)-rot_mat).norm(), 0.0, 1e-08);\n        }\n        \n        ASSERT_NEAR((GammaFunc(Eigen::Vector3d::Zero(), 1)-Eigen::Matrix3d::Identity()).norm(), 0.0, 1e-08);\n        \n        ASSERT_NEAR((GammaFunc(Eigen::Vector3d::Zero(), 2)-0.5*Eigen::Matrix3d::Identity()).norm(), 0.0, 1e-08);\n        \n        ASSERT_NEAR((GammaFunc(Eigen::Vector3d::Zero(), 3)-1.0/6.0*Eigen::Matrix3d::Identity()).norm(), 0.0, 1e-08);\n    }\n\n    TEST(testState, StateAddMargProp)\n    {\n        IngvioParams filter_params;\n        \n        filter_params.readParams(\"/home/lcw/VIO/ws_ingvio/src/config/sportsfield/ingvio_stereo.yaml\");\n        filter_params._enable_gnss = 1;\n        filter_params.printParams();\n        \n        \n        std::shared_ptr<State> state = std::make_shared<State>(filter_params);\n        \n        ASSERT_TRUE(StateManager::checkStateContinuity(state));\n        ASSERT_EQ(state->curr_cov_size(), 21);\n        \n        ASSERT_NEAR((state->_camleft_imu_extrinsics->valueLinearAsMat()-filter_params._T_cl2i.linear()).norm(), 0.0, 1e-08);\n        \n        ASSERT_NEAR((state->_camleft_imu_extrinsics->valueTrans()-filter_params._T_cl2i.translation()).norm(), 0.0, 1e-08);\n        \n        StateManager::addGNSSVariable(state, State::GNSSType::GPS, 20.0, 4.0);\n        ASSERT_TRUE(state->curr_cov_size() == 22);\n        ASSERT_TRUE(state->curr_err_variable_size() == 5);\n        \n        StateManager::addGNSSVariable(state, State::GNSSType::BDS, 16.0, 4.0);\n        ASSERT_TRUE(state->curr_cov_size() == 23);\n        ASSERT_TRUE(state->curr_err_variable_size() == 6);\n        \n        StateManager::addGNSSVariable(state, State::GNSSType::YOF, 123.0, 1.0);\n        ASSERT_TRUE(state->curr_cov_size() == 24);\n        ASSERT_TRUE(state->curr_err_variable_size() == 7);\n        \n        StateManager::addGNSSVariable(state, State::GNSSType::FS, 2.0, 1.0);\n        ASSERT_TRUE(state->curr_cov_size() == 25);\n        ASSERT_TRUE(state->curr_err_variable_size() == 8);\n        \n        StateManager::margGNSSVariable(state, State::GNSSType::GPS);\n        ASSERT_TRUE(state->curr_cov_size() == 24);\n        ASSERT_TRUE(state->curr_err_variable_size() == 7);\n        \n        StateManager::addGNSSVariable(state, State::GNSSType::GLO, 3.0, 6.0);\n        ASSERT_TRUE(state->curr_cov_size() == 25);\n        ASSERT_TRUE(state->curr_err_variable_size() == 8);\n        \n        Eigen::MatrixXd cov = StateManager::getFullCov(state);\n        const Eigen::Matrix<double, 15, 15> Phi_imu = Eigen::Matrix<double, 15, 15>::Random();\n        const Eigen::Matrix<double, 15, 12> G_imu = Eigen::Matrix<double, 15, 12>::Random();\n        const double dt = 1.5;\n        \n        Eigen::Matrix<double, 12, 12> Q_imu;\n        Q_imu.setIdentity();\n        \n        Q_imu.middleCols<3>(0) *= std::pow(state->_state_params._noise_g, 2.0);\n        Q_imu.middleCols<3>(3) *= std::pow(state->_state_params._noise_a, 2.0);\n        Q_imu.middleCols<3>(6) *= std::pow(state->_state_params._noise_bg, 2.0);\n        Q_imu.middleCols<3>(9) *= std::pow(state->_state_params._noise_ba, 2.0);\n        \n        Eigen::Matrix2d Q_gnss;\n        Q_gnss.setZero();\n        Q_gnss(0, 0) = std::pow(state->_state_params._noise_clockbias, 2.0);\n        Q_gnss(1, 1) = std::pow(state->_state_params._noise_cb_rw, 2.0);\n        \n        Eigen::Matrix<double, 14, 14> Q = Eigen::Matrix<double, 14, 14>::Zero();\n        Q.block<12, 12>(0, 0) = Q_imu;\n        Q.block<2, 2>(12, 12) = Q_gnss;\n        \n        Eigen::Matrix4d Phi_gnss = Eigen::Matrix4d::Identity();\n        Phi_gnss(0, 2) = dt;\n        Phi_gnss(3, 2) = dt;\n        \n        Eigen::Matrix<double, 25, 25> Phi = Eigen::Matrix<double, 25, 25>::Identity();\n        Phi.block<15, 15>(0, 0) = Phi_imu;\n        Phi.block<4, 4>(21, 21) = Phi_gnss;\n        \n        Eigen::Matrix<double, 25, 14> G = Eigen::Matrix<double, 25, 14>::Zero();\n        G.block<15, 12>(0, 0) = G_imu;\n        Eigen::Matrix<double, 4, 2> G_gnss = Eigen::Matrix<double, 4, 2>::Zero();\n        G_gnss(0, 0) = 1;\n        G_gnss(2, 1) = 1;\n        G_gnss(3, 0) = 1;\n        G.block<4, 2>(21, 12) = G_gnss;\n        \n        Eigen::MatrixXd cov_result = Phi*cov*Phi.transpose() + dt*Phi*G*Q*G.transpose()*Phi.transpose();\n        \n        StateManager::propagateStateCov(state, Phi_imu, G_imu, dt);\n        \n        ASSERT_NEAR((StateManager::getFullCov(state)-cov_result).norm(), 0.0, 1e-10);\n        \n        // std::cout << \"propagateStateCov = \" << StateManager::getFullCov(state) << std::endl;\n        // std::cout << \"=====================\" << std::endl;\n        // std::cout << \"calculated ref cov = \" << cov_result << std::endl;\n        \n        std::vector<std::shared_ptr<Type>> small_var;\n        small_var.push_back(state->_extended_pose);\n\n        small_var.push_back(state->_ba);\n    \n        small_var.push_back(state->_gnss.at(State::GNSSType::YOF));\n        \n        small_var.push_back(state->_gnss.at(State::GNSSType::GLO));\n        \n        Eigen::MatrixXd small_cov = StateManager::getMarginalCov(state, small_var);\n    \n        ASSERT_NEAR((small_cov.block<9, 9>(0, 0) - StateManager::getFullCov(state).block<9, 9>(0, 0)).norm(), 0.0, 1e-06);\n        ASSERT_NEAR((small_cov.block<3, 3>(9, 9) - StateManager::getFullCov(state).block<3, 3>(12, 12)).norm(), 0.0, 1e-06);\n    }\n\n    class StateUpdateTest : public virtual ::testing::Test\n    {\n    protected:\n        StateUpdateTest()\n        {\n            filter_params.readParams(\"/home/lcw/VIO/ws_ingvio/src/config/sportsfield/ingvio_stereo.yaml\");\n            \n            state = std::make_shared<State>(filter_params);\n            \n            state->_extended_pose->setRandom();\n            state->_bg->setRandom();\n            state->_ba->setRandom();\n            \n            state->_camleft_imu_extrinsics->setRandom();\n            \n            StateManager::addGNSSVariable(state, State::GNSSType::GPS, 20.0, 4.0);\n            \n            StateManager::addGNSSVariable(state, State::GNSSType::YOF, 123.0, 1.0);\n            \n            StateManager::addGNSSVariable(state, State::GNSSType::FS, 2.0, 1.0);\n            \n            StateManager::addGNSSVariable(state, State::GNSSType::BDS, 16.0, 4.0);\n            \n            Phi_imu.setRandom();\n            \n            G_imu.setRandom();\n        }\n        \n        virtual ~StateUpdateTest() {}\n        \n        IngvioParams filter_params;\n        std::shared_ptr<State> state;\n        \n        Eigen::Matrix<double, 15, 15> Phi_imu;\n        Eigen::Matrix<double, 15, 12> G_imu;\n    };\n    \n    TEST_F(StateUpdateTest, augmentPose)\n    {\n        auto constructJ = [](const Eigen::Matrix3d& C_i2w)\n        {\n            Eigen::Matrix<double, 6, 21> tmp;\n            tmp.setZero();\n            \n            tmp.block<3, 3>(0, 0).setIdentity();\n            tmp.block<3, 3>(3, 3).setIdentity();\n            \n            tmp.block<3, 3>(0, 15) = C_i2w;\n            tmp.block<3, 3>(3, 18) = C_i2w;\n            \n            return tmp;\n        };\n        \n        auto constructLargeJ = [&](int orig_row, const Eigen::Matrix3d& C_i2w)\n        {\n            Eigen::MatrixXd tmp(orig_row+6, orig_row);\n            tmp.setZero();\n            \n            tmp.block(0, 0, orig_row, orig_row) = Eigen::MatrixXd::Identity(orig_row, orig_row);\n            tmp.block<6, 21>(orig_row, 0) = constructJ(C_i2w);\n            \n            return tmp;\n        };\n        \n        this->state->_timestamp = 1.0;\n        \n        StateManager::propagateStateCov(this->state, this->Phi_imu, this->G_imu, 1.5);\n        \n        state->_timestamp = 2.5;\n        \n        Eigen::MatrixXd cov1 = StateManager::getFullCov(state);\n        Eigen::MatrixXd largeJ1 = constructLargeJ(cov1.rows(), this->state->_extended_pose->valueLinearAsMat()); \n        \n        StateManager::augmentSlidingWindowPose(state);\n        \n        ASSERT_NEAR((StateManager::getFullCov(state)-largeJ1*cov1*largeJ1.transpose()).norm(), 0.0, 1e-08);\n        \n        ASSERT_EQ(state->curr_cov_size(), cov1.rows() + 6);\n        \n        ASSERT_NEAR((state->_extended_pose->valueLinearAsMat()*state->_camleft_imu_extrinsics->valueLinearAsMat()-state->_sw_camleft_poses[2.5]->valueLinearAsMat()).norm(), 0.0, 1e-08);\n        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);\n        \n        StateManager::propagateStateCov(this->state, this->Phi_imu, this->G_imu, 0.5);\n        \n        state->_timestamp = 3.0;\n        \n        Eigen::MatrixXd cov2 = StateManager::getFullCov(state);\n        Eigen::MatrixXd largeJ2 = constructLargeJ(cov2.rows(), this->state->_extended_pose->valueLinearAsMat());\n        \n        StateManager::augmentSlidingWindowPose(state);\n        \n        ASSERT_NEAR((StateManager::getFullCov(state)-largeJ2*cov2*largeJ2.transpose()).norm(), 0.0, 1e-08);\n        \n        ASSERT_EQ(state->curr_cov_size(), cov2.rows() + 6);\n        \n        ASSERT_NEAR((state->_extended_pose->valueLinearAsMat()*state->_camleft_imu_extrinsics->valueLinearAsMat()-state->_sw_camleft_poses[3.0]->valueLinearAsMat()).norm(), 0.0, 1e-08);\n        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);\n    }\n    \n    TEST_F(StateUpdateTest, stateBoxPlus)\n    {\n        \n        Eigen::Vector3d x1, x2, x3;\n        x1.setRandom();\n        x1.z() = std::fabs(x1.z());\n        x1.head<2>().normalize();\n        AnchoredLandmark::switchRepBear2Xyz(x1, x2);\n        AnchoredLandmark::switchRepXyz2Bear(x2, x3);\n        ASSERT_NEAR(x1.z()-x3.z(), 0.0, 1e-08);\n        ASSERT_NEAR(std::fabs(x1.y()), std::fabs(x3.y()), 1e-08);\n        ASSERT_TRUE(std::fabs(x1.x()-x3.x()) < 1e-08 || std::fabs(std::fabs(x1.x()-x3.x())-M_PI) < 1e-08);\n        \n        x1.setRandom();\n        x1.z() = std::fabs(x1.z());\n        AnchoredLandmark::switchRepXyz2Bear(x1, x2);\n        AnchoredLandmark::switchRepBear2Xyz(x2, x3);\n        ASSERT_NEAR((x1-x3).norm(), 0.0, 1e-08);\n        \n        x1.setRandom();\n        x1.z() = std::fabs(x1.z());\n        AnchoredLandmark::switchRepXyz2Invd(x1, x2);\n        AnchoredLandmark::switchRepInvd2Xyz(x2, x3);\n        ASSERT_NEAR((x1-x3).norm(), 0.0, 1e-08);\n        \n        x1.setRandom();\n        x1.z() = std::fabs(x1.z());\n        AnchoredLandmark::switchRepInvd2Xyz(x1, x2);\n        AnchoredLandmark::switchRepXyz2Invd(x2, x3);\n        ASSERT_NEAR((x1-x3).norm(), 0.0, 1e-08);\n        \n        this->state->_timestamp = 1.0;\n        \n        StateManager::propagateStateCov(this->state, this->Phi_imu, this->G_imu, 1.5);\n        \n        state->_timestamp = 2.5;\n        \n        StateManager::augmentSlidingWindowPose(state);\n        \n        std::shared_ptr<AnchoredLandmark> lm1(new AnchoredLandmark());\n        \n        lm1->resetAnchoredPose(state->_sw_camleft_poses.at(2.5));\n        lm1->setRepType(AnchoredLandmark::LmRep::BEARING);\n        \n        Eigen::Vector3d tmp = Eigen::Vector3d::Random();\n        tmp.z() = std::fabs(tmp.z());\n        tmp.topRows<2>().normalize();\n        lm1->setValuePosRep(tmp);\n        \n        StateManager::addAnchoredLandmarkInState(state, lm1, 5, 10.0*Eigen::Matrix3d::Identity());\n        \n        Eigen::Vector3d bodyXYZ;\n        bodyXYZ.z() = 1.0/tmp.z()*std::cos(tmp.y());\n        bodyXYZ.x() = 1.0/tmp.z()*std::cos(tmp.x())*std::sin(tmp.y());\n        bodyXYZ.y() = 1.0/tmp.z()*std::sin(tmp.x())*std::sin(tmp.y());\n        \n        Eigen::Vector3d worldXYZ = state->_sw_camleft_poses.at(2.5)->valueLinearAsMat()*bodyXYZ + state->_sw_camleft_poses.at(2.5)->valueTrans();\n        \n        ASSERT_NEAR((worldXYZ-lm1->valuePosXyz()).norm(), 0.0, 1e-8);\n        ASSERT_NEAR((worldXYZ-state->_anchored_landmarks.at(5)->valuePosXyz()).norm(), 0.0, 1e-08);\n        \n        StateManager::propagateStateCov(this->state, this->Phi_imu, this->G_imu, 0.5);\n        \n        state->_timestamp = 3.0;\n        \n        StateManager::augmentSlidingWindowPose(state);\n        \n        std::shared_ptr<AnchoredLandmark> lm2(new AnchoredLandmark());\n        \n        lm2->resetAnchoredPose(state->_sw_camleft_poses.at(3.0));\n        lm2->setRepType(AnchoredLandmark::LmRep::INV_DEPTH);\n        \n        tmp = Eigen::Vector3d::Random();\n        tmp.z() = std::fabs(tmp.z());\n        lm2->setValuePosRep(tmp);\n        \n        StateManager::addAnchoredLandmarkInState(state, lm2, 6, 12.0*Eigen::Matrix3d::Identity());\n        \n        bodyXYZ.x() = tmp.x()/tmp.z();\n        bodyXYZ.y() = tmp.y()/tmp.z();\n        bodyXYZ.z() = 1.0/tmp.z();\n        \n        worldXYZ = state->_sw_camleft_poses.at(3.0)->copyValueAsIso()*bodyXYZ;\n        ASSERT_NEAR((worldXYZ-lm2->valuePosXyz()).norm(), 0.0, 1e-08);\n        ASSERT_NEAR((worldXYZ-state->_anchored_landmarks.at(6)->valuePosXyz()).norm(), 0.0, 1e-08);\n        \n        Eigen::Matrix3d R_hat = state->_extended_pose->valueLinearAsMat();\n        Eigen::Vector3d p_hat = state->_extended_pose->valueTrans1();\n        Eigen::Vector3d v_hat = state->_extended_pose->valueTrans2();\n\n        Eigen::Vector3d bg_hat = state->_bg->value();\n        Eigen::Vector3d ba_hat = state->_ba->value();\n\n        Eigen::Matrix3d R_ext_hat = state->_camleft_imu_extrinsics->valueLinearAsMat();\n        Eigen::Vector3d p_ext_hat = state->_camleft_imu_extrinsics->valueTrans();\n        \n        double t_gps_hat = state->_gnss.at(State::GNSSType::GPS)->value();\n        double t_bds_hat = state->_gnss.at(State::GNSSType::BDS)->value();\n        double fs_hat = state->_gnss.at(State::GNSSType::FS)->value();\n        double yof_hat = state->_gnss.at(State::GNSSType::YOF)->value();\n        \n        Eigen::Matrix3d R_c1_hat = state->_sw_camleft_poses.at(2.5)->valueLinearAsMat();\n        Eigen::Vector3d p_c1_hat = state->_sw_camleft_poses.at(2.5)->valueTrans();\n        \n        Eigen::Matrix3d R_c2_hat = state->_sw_camleft_poses.at(3.0)->valueLinearAsMat();\n        Eigen::Vector3d p_c2_hat = state->_sw_camleft_poses.at(3.0)->valueTrans();\n        \n        Eigen::Vector3d pf1_hat = state->_anchored_landmarks.at(5)->valuePosXyz();\n        Eigen::Vector3d pf2_hat = state->_anchored_landmarks.at(6)->valuePosXyz();\n        \n        const int rows = state->curr_cov_size();\n        Eigen::VectorXd dx(rows);\n        dx.setRandom();\n        \n        const Eigen::Vector3d& delta_theta = dx.block<3, 1>(0, 0);\n        const Eigen::Vector3d& delta_p = dx.block<3, 1>(3, 0);\n        const Eigen::Vector3d& delta_v = dx.block<3, 1>(6, 0);\n       \n        const Eigen::Vector3d& delta_bg = dx.block<3, 1>(9, 0);\n        const Eigen::Vector3d& delta_ba = dx.block<3, 1>(12, 0);\n        \n        const Eigen::Vector3d& delta_theta_ext = dx.block<3, 1>(15, 0);\n        const Eigen::Vector3d& delta_p_ext = dx.block<3, 1>(18, 0);\n        \n        const double& delta_t_gps = dx(21);\n        const double& delta_yof = dx(22);\n        const double& delta_fs = dx(23);\n        const double& delta_t_bds = dx(24);\n        \n        const Eigen::Vector3d& delta_theta_c1 = dx.block<3, 1>(25, 0);\n        const Eigen::Vector3d& delta_p_c1 = dx.block<3, 1>(28, 0);\n        \n        const Eigen::Vector3d& delta_pf1 = dx.block<3, 1>(31, 0);\n        \n        const Eigen::Vector3d& delta_theta_c2 = dx.block<3, 1>(34, 0);\n        const Eigen::Vector3d& delta_p_c2 = dx.block<3, 1>(37, 0);\n        \n        const Eigen::Vector3d& delta_pf2 = dx.block<3, 1>(40, 0);\n        \n        Eigen::Matrix3d R = GammaFunc(delta_theta, 0)*R_hat;\n        Eigen::Vector3d p = GammaFunc(delta_theta, 0)*p_hat + GammaFunc(delta_theta, 1)*delta_p;\n        Eigen::Vector3d v = GammaFunc(delta_theta, 0)*v_hat + GammaFunc(delta_theta, 1)*delta_v;\n        \n        Eigen::Vector3d bg = bg_hat + delta_bg;\n        Eigen::Vector3d ba = ba_hat + delta_ba;\n        \n        Eigen::Matrix3d R_ext = GammaFunc(delta_theta_ext, 0)*R_ext_hat;\n        Eigen::Vector3d p_ext = GammaFunc(delta_theta_ext, 0)*p_ext_hat + GammaFunc(delta_theta_ext, 1)*delta_p_ext;\n        \n        double t_gps = t_gps_hat + delta_t_gps;\n        double t_bds = t_bds_hat + delta_t_bds;\n        double yof = yof_hat + delta_yof;\n        double fs = fs_hat + delta_fs;\n        \n        Eigen::Matrix3d R_c1 = GammaFunc(delta_theta_c1, 0)*R_c1_hat;\n        Eigen::Vector3d p_c1 = GammaFunc(delta_theta_c1, 0)*p_c1_hat + GammaFunc(delta_theta_c1, 1)*delta_p_c1;\n        \n        Eigen::Matrix3d R_c2 = GammaFunc(delta_theta_c2, 0)*R_c2_hat;\n        Eigen::Vector3d p_c2 = GammaFunc(delta_theta_c2, 0)*p_c2_hat + GammaFunc(delta_theta_c2, 1)*delta_p_c2;\n        \n        Eigen::Vector3d pf1 = GammaFunc(delta_theta_c1, 0)*pf1_hat + GammaFunc(delta_theta_c1, 1)*delta_pf1;\n        \n        Eigen::Vector3d pf2 = GammaFunc(delta_theta_c2, 0)*pf2_hat + GammaFunc(delta_theta_c2, 1)*delta_pf2;\n        \n        StateManager::boxPlus(state, dx);\n        \n        auto isMatNear = [](const Eigen::MatrixXd& mat1, const Eigen::MatrixXd& mat2)\n        {\n            assert(mat1.rows() == mat2.rows());\n            assert(mat1.cols() == mat2.cols());\n            \n            if ((mat1-mat2).norm() < 1e-08)\n                return true;\n            else return false;\n        };\n        \n        ASSERT_TRUE(isMatNear(R, state->_extended_pose->valueLinearAsMat()));\n        ASSERT_TRUE(isMatNear(p, state->_extended_pose->valueTrans1()));\n        ASSERT_TRUE(isMatNear(v, state->_extended_pose->valueTrans2()));\n        \n        ASSERT_TRUE(isMatNear(bg, state->_bg->value()));\n        ASSERT_TRUE(isMatNear(ba, state->_ba->value()));\n        \n        ASSERT_TRUE(isMatNear(R_ext, state->_camleft_imu_extrinsics->valueLinearAsMat()));\n        ASSERT_TRUE(isMatNear(p_ext, state->_camleft_imu_extrinsics->valueTrans()));\n        \n        ASSERT_NEAR(t_gps, state->_gnss.at(State::GNSSType::GPS)->value(), 1e-08);\n        ASSERT_NEAR(t_bds, state->_gnss.at(State::GNSSType::BDS)->value(), 1e-08);\n        ASSERT_NEAR(yof, state->_gnss.at(State::GNSSType::YOF)->value(), 1e-08);\n        ASSERT_NEAR(fs, state->_gnss.at(State::GNSSType::FS)->value(), 1e-08);\n        \n        ASSERT_TRUE(isMatNear(R_c1, state->_sw_camleft_poses.at(2.5)->valueLinearAsMat()));\n        ASSERT_TRUE(isMatNear(p_c1, state->_sw_camleft_poses.at(2.5)->valueTrans()));\n        \n        ASSERT_TRUE(isMatNear(R_c2, state->_sw_camleft_poses.at(3.0)->valueLinearAsMat()));\n        ASSERT_TRUE(isMatNear(p_c2, state->_sw_camleft_poses.at(3.0)->valueTrans()));\n        \n        ASSERT_TRUE(isMatNear(pf1, state->_anchored_landmarks.at(5)->valuePosXyz()));\n        ASSERT_TRUE(isMatNear(pf2, state->_anchored_landmarks.at(6)->valuePosXyz()));\n        \n        pf1 += delta_pf1;\n        pf2 += delta_pf2;\n        \n        state->_anchored_landmarks.at(5)->resetAnchoredPose();\n        state->_anchored_landmarks.at(6)->resetAnchoredPose();\n        \n        StateManager::boxPlus(state, dx);\n        \n        ASSERT_TRUE(isMatNear(pf1, state->_anchored_landmarks.at(5)->valuePosXyz()));\n        ASSERT_TRUE(isMatNear(pf2, state->_anchored_landmarks.at(6)->valuePosXyz()));\n        \n        StateManager::margAnchoredLandmarkInState(state, 5);\n        StateManager::margAnchoredLandmarkInState(state, 6);\n        StateManager::margSlidingWindowPose(state);\n        StateManager::margSlidingWindowPose(state, 3.0);\n        StateManager::margSlidingWindowPose(state, 2.5);\n        \n        ASSERT_TRUE(state->curr_cov_size() == 25);\n        ASSERT_TRUE(StateManager::checkStateContinuity(state));\n    }\n    \n    TEST_F(StateUpdateTest, stateCovUpdate)\n    {\n        this->state->_timestamp = 1.0;\n        \n        StateManager::propagateStateCov(this->state, this->Phi_imu, this->G_imu, 1.5);\n        \n        state->_timestamp = 2.5;\n        \n        StateManager::augmentSlidingWindowPose(state);\n        \n        std::shared_ptr<AnchoredLandmark> lm1(new AnchoredLandmark());\n        \n        lm1->resetAnchoredPose(state->_sw_camleft_poses.at(2.5));\n        lm1->setRepType(AnchoredLandmark::LmRep::BEARING);\n        \n        Eigen::Vector3d tmp = Eigen::Vector3d::Random();\n        tmp.z() = std::fabs(tmp.z());\n        tmp.topRows<2>().normalize();\n        lm1->setValuePosRep(tmp);\n        \n        StateManager::addAnchoredLandmarkInState(state, lm1, 5, 10.0*Eigen::Matrix3d::Identity());\n        \n        std::vector<std::shared_ptr<Type>> var_order;\n        var_order.push_back(state->_extended_pose);\n        var_order.push_back(state->_gnss.at(State::GNSSType::GPS));\n        var_order.push_back(state->_gnss.at(State::GNSSType::BDS));\n        var_order.push_back(state->_gnss.at(State::GNSSType::FS));\n        \n        int var_size = StateManager::calcSubVarSize(var_order);\n        \n        Eigen::VectorXd res(6);\n        res.setRandom();\n        \n        Eigen::MatrixXd H(res.rows(), var_size);\n        H.setZero();\n        \n        H.block<6, 3>(0, 0).setRandom();\n        H.block<3, 3>(0, 3).setRandom();\n        H.block<3, 3>(3, 6).setRandom();\n        \n        H(0, 9) = 1.0;\n        H(1, 9) = 1.0;\n        H(2, 10) = 1.0;\n        \n        H.block<3, 1>(3, 11).setOnes();\n        \n        Eigen::MatrixXd H_large(res.rows(), state->curr_cov_size());\n        \n        H_large.setZero();\n        \n        H_large.block<6, 9>(0, 0) = H.block<6, 9>(0, 0);\n        \n        H_large(0, state->_gnss.at(State::GNSSType::GPS)->idx()) = 1.0;\n        H_large(1, state->_gnss.at(State::GNSSType::GPS)->idx()) = 1.0;\n        H_large(2, state->_gnss.at(State::GNSSType::BDS)->idx()) = 1.0;\n        \n        H_large.block<3, 1>(3, state->_gnss.at(State::GNSSType::FS)->idx()).setOnes();\n        \n        Eigen::MatrixXd cov_orig = StateManager::getFullCov(state);\n        \n        auto isMatNear = [](const Eigen::MatrixXd& mat1, const Eigen::MatrixXd& mat2)\n        {\n            assert(mat1.rows() == mat2.rows());\n            assert(mat1.cols() == mat2.cols());\n            \n            if ((mat1-mat2).norm() < 1e-08)\n                return true;\n            else return false;\n        };\n        \n        Eigen::MatrixXd R = 0.5*Eigen::MatrixXd::Identity(res.rows(), res.rows());\n        \n        StateManager::ekfUpdate(state, var_order, H, res, R);\n        \n        Eigen::MatrixXd K = cov_orig*H_large.transpose()*(H_large*cov_orig*H_large.transpose() + R).inverse();\n        \n        Eigen::MatrixXd cov_ref = (Eigen::MatrixXd::Identity(cov_orig.rows(), cov_orig.cols())-K*H_large)*cov_orig;\n        \n        ASSERT_TRUE(isMatNear(cov_ref, StateManager::getFullCov(state)));\n    }\n    \n    class AddDelayedTest : public virtual ::testing::Test\n    {\n    protected:\n        AddDelayedTest()\n        {\n            filter_params.readParams(\"/home/lcw/VIO/ws_ingvio/src/config/sportsfield/ingvio_stereo.yaml\");\n            \n            state = std::make_shared<State>(filter_params);\n            \n            state->_extended_pose->setRandom();\n            state->_bg->setRandom();\n            state->_ba->setRandom();\n            \n            state->_camleft_imu_extrinsics->setRandom();\n            \n            Phi_imu.setRandom();\n            G_imu.setRandom();\n            \n            state->initStateAndCov(1.0, Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::Random().normalized(), Eigen::Vector3d::Random().normalized()));\n            \n            this->state->_timestamp = 1.0;\n            StateManager::propagateStateCov(this->state, this->Phi_imu, this->G_imu, 1.5);\n            state->_timestamp = 2.5;\n        }\n        \n        virtual ~AddDelayedTest() {}\n        \n        IngvioParams filter_params;\n        std::shared_ptr<State> state;\n        \n        Eigen::Matrix<double, 15, 15> Phi_imu;\n        Eigen::Matrix<double, 15, 12> G_imu;\n    \n    };\n\n    TEST_F(AddDelayedTest, addVarInv)\n    {\n        Eigen::VectorXd res(1);\n        res = Eigen::Vector2d::Random().head<1>();\n        \n        std::vector<std::shared_ptr<Type>> sub_var_old;\n        sub_var_old.push_back(state->_extended_pose);\n        \n        Eigen::MatrixXd H_old(1, state->_extended_pose->size());\n        H_old.setRandom();\n        \n        Eigen::MatrixXd H_new(1, 1);\n        H_new(0, 0) = 1.0;\n        \n        std::shared_ptr<Scalar> tgps = std::make_shared<Scalar>();\n        tgps->setRandom();\n        state->_gnss[State::GNSSType::GPS] = tgps;\n        \n        Eigen::MatrixXd cov_orig = StateManager::getFullCov(state);\n        \n        double noise_meas_iso = 2.0;\n        \n        StateManager::addVariableDelayedInvertible(state, tgps, sub_var_old, H_old, H_new, res, noise_meas_iso);\n        \n        ASSERT_TRUE(StateManager::checkStateContinuity(state));\n        \n        ASSERT_EQ(state->curr_cov_size(), cov_orig.cols()+1);\n        \n        ASSERT_EQ(state->_gnss[State::GNSSType::GPS]->idx(), cov_orig.rows());\n        \n        Eigen::MatrixXd cov_new = StateManager::getFullCov(state);\n        \n        Eigen::MatrixXd x1 = H_old*cov_orig.block<9, 9>(0, 0)*H_old.transpose();\n        \n        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);\n        \n        auto isMatNear = [](const Eigen::MatrixXd& mat1, const Eigen::MatrixXd& mat2)\n        {\n            if ((mat1-mat2).norm() < 1e-08)\n                return true;\n            else\n                return false;\n        };\n        \n        Eigen::Matrix<double, 1, 21> H_old_large = Eigen::Matrix<double, 1, 21>::Zero();\n        H_old_large.block<1, 9>(0, 0) = H_old.block<1, 9>(0, 0);\n        \n        ASSERT_TRUE(isMatNear(cov_new.block<21, 1>(0, 21), -cov_orig*H_old_large.transpose()/H_new(0, 0)));\n    }\n    \n    TEST_F(AddDelayedTest, addVar)\n    {\n        Eigen::VectorXd res(2);\n        res.setRandom();\n        \n        std::vector<std::shared_ptr<Type>> sub_var_old;\n        sub_var_old.push_back(state->_extended_pose);\n        \n        Eigen::MatrixXd H_old(2, state->_extended_pose->size());\n        H_old.setRandom();\n        \n        Eigen::MatrixXd H_new(2, 1);\n        H_new(0, 0) = 1.0;\n        H_new(1, 0) = 1.0;\n        \n        Eigen::MatrixXd H_old_copy = H_old;\n        Eigen::MatrixXd H_new_copy = H_new;\n        Eigen::VectorXd res_copy = res;\n        \n        std::shared_ptr<Scalar> tgps = std::make_shared<Scalar>();\n        tgps->setRandom();\n        state->_gnss[State::GNSSType::GPS] = tgps;\n        \n        Eigen::MatrixXd cov_orig = StateManager::getFullCov(state);\n        \n        double noise_meas_iso = 2.0;\n        \n        StateManager::addVariableDelayed(state, tgps, sub_var_old, H_old, H_new, res, noise_meas_iso, 1.0, false);\n        \n        ASSERT_TRUE(StateManager::checkStateContinuity(state));\n        \n        ASSERT_EQ(state->curr_cov_size(), cov_orig.cols()+1);\n        \n        ASSERT_EQ(state->_gnss[State::GNSSType::GPS]->idx(), cov_orig.rows());\n        \n        Eigen::MatrixXd cov_new = StateManager::getFullCov(state);\n        \n        Eigen::Matrix<double, 2, 2> Q_T;\n        Q_T(0, 0) = std::sqrt(2.0)/2.0;\n        Q_T(0, 1) = std::sqrt(2.0)/2.0;\n        Q_T(1, 0) = -std::sqrt(2.0)/2.0;\n        Q_T(1, 1) = std::sqrt(2.0)/2.0;\n        \n        res_copy = Q_T*res_copy;\n        H_old_copy = Q_T*H_old_copy;\n        H_new_copy = Q_T*H_new_copy;\n        \n        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();\n    \n        \n        auto isMatNear = [](const Eigen::MatrixXd& mat1, const Eigen::MatrixXd& mat2)\n        {\n            if ((mat1-mat2).norm() < 1e-08)\n                return true;\n            else\n                return false;\n        };\n        \n        Eigen::MatrixXd cov_before_update(22, 22);\n        cov_before_update.block<21, 21>(0, 0) = cov_orig;\n        \n        cov_before_update(21, 21) = (x1(0, 0)+std::pow(noise_meas_iso, 2.0))/std::pow(H_new_copy(0, 0), 2.0);\n        \n        Eigen::Matrix<double, 1, 21> H_old_copy_large = Eigen::Matrix<double, 1, 21>::Zero();\n        H_old_copy_large.block<1, 9>(0, 0) = H_old_copy.block<1, 9>(0, 0);\n        \n        cov_before_update.block<21, 1>(0, 21) = -cov_orig*H_old_copy_large.transpose()/H_new_copy(0, 0);\n        \n        cov_before_update.block<1, 21>(21, 0) = cov_before_update.block<21, 1>(0, 21).transpose();\n        \n        Eigen::MatrixXd H_update = Eigen::MatrixXd::Zero(1, 22);\n        H_update.block<1, 9>(0, 0) = H_old_copy.block<1, 9>(1, 0);\n        \n        Eigen::MatrixXd S = H_update*cov_before_update*H_update.transpose() + std::pow(noise_meas_iso, 2.0)*Eigen::MatrixXd::Identity(1, 1);\n        \n        Eigen::MatrixXd Kalman_gain = cov_before_update*H_update.transpose()*S.inverse();\n        \n        Eigen::MatrixXd cov_after_update = cov_before_update;\n        cov_after_update -= Kalman_gain*H_update*cov_before_update;\n        \n        ASSERT_TRUE(isMatNear(cov_after_update, cov_new));\n    }\n    \n}\n\nint main(int argc, char** argv)\n{\n    testing::InitGoogleTest(&argc, argv);\n    return RUN_ALL_TESTS();\n}\n"
  },
  {
    "path": "ingvio_estimator/test/TestTriangulator.cpp",
    "content": "/**  This File is part of InGVIO, an invariant filter for mono/stereo visual-\n *    inertial-raw GNSS navigation. \n *    \n *    Copyright (C) 2022  Changwu Liu (cwliu529@163.com,\n *                                     lcw18@mails.tsinghua.edu.cn (valid until 2023))\n *    \n *    This program is free software: you can redistribute it and/or modify\n *    it under the terms of the GNU General Public License as published by\n *    the Free Software Foundation, either version 3 of the License, or\n *    (at your option) any later version.\n *    \n *    This program is distributed in the hope that it will be useful,\n *    but WITHOUT ANY WARRANTY; without even the implied warranty of\n *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\n *    GNU General Public License for more details.\n *    \n *    You should have received a copy of the GNU General Public License\n *    along with this program.  If not, see <https://www.gnu.org/licenses/>.\n */\n\n#include <gtest/gtest.h>\n\n#include <cmath>\n#include \"MapServer.h\"\n#include \"Triangulator.h\"\n#include \"GenerateNoise.h\"\n\nusing namespace ingvio;\n\nnamespace ingvio_test\n{    \n    class TestTriangulator : public virtual ::testing::Test\n    {\n    public:\n        TestTriangulator() : pf(Eigen::Vector3d(1.0, 2.0, 3.0))\n        {\n            T_cl2cr.linear() = Eigen::Matrix3d::Identity();\n            T_cl2cr.translation() = Eigen::Vector3d(0.001, -0.12, 0.003);\n            \n            for (int i = 0; i < 10; ++i)\n            {\n                sw_poses1[0.1*i] = std::make_shared<SE3>();\n                \n                sw_poses1[0.1*i]->setValueLinearByMat(Eigen::AngleAxisd(generateGaussRandom(0.0, 0.1), Eigen::Vector3d(0, 0, 1)).toRotationMatrix());\n                \n                sw_poses1[0.1*i]->setValueTrans(Eigen::Vector3d(2*i-9.0, 2*i-9.0, 0.0));\n                \n                mono_obs1[0.1*i] = calcMonoMeas(pf, sw_poses1.at(0.1*i));\n                \n                stereo_obs1[0.1*i] = calcStereoMeas(pf, sw_poses1.at(0.1*i));\n            }\n            \n            sw_poses2[0.1] = std::make_shared<SE3>();\n            sw_poses2[0.1]->setValueLinearByQuat(Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d(0, 0, 1), Eigen::Vector3d(0, 0, 1)));\n            sw_poses2[0.1]->setValueTrans(Eigen::Vector3d(0, 0, 0));\n            mono_obs2[0.1] = calcMonoMeas(pf, sw_poses2.at(0.1));\n            stereo_obs2[0.1] = calcStereoMeas(pf, sw_poses2.at(0.1));\n            \n            sw_poses2[0.2] = std::make_shared<SE3>();\n            sw_poses2[0.2]->setValueLinearByQuat(Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d(0, 0, 1), Eigen::Vector3d(0, -1, 0)));\n            sw_poses2[0.2]->setValueTrans(Eigen::Vector3d(0, 5, 0));\n            mono_obs2[0.2] = calcMonoMeas(pf, sw_poses2.at(0.2));\n            stereo_obs2[0.2] = calcStereoMeas(pf, sw_poses2.at(0.2));\n            \n            sw_poses2[0.3] = std::make_shared<SE3>();\n            sw_poses2[0.3]->setValueLinearByQuat(Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d(0, 0, 1), Eigen::Vector3d(0, 0, -1)));\n            sw_poses2[0.3]->setValueTrans(Eigen::Vector3d(0, 0, 8));\n            mono_obs2[0.3] = calcMonoMeas(pf, sw_poses2.at(0.3));\n            stereo_obs2[0.3] = calcStereoMeas(pf, sw_poses2.at(0.3));\n            \n            sw_poses2[0.4] = std::make_shared<SE3>();\n            sw_poses2[0.4]->setValueLinearByQuat(Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d(0, 0, 1), Eigen::Vector3d(0, 1, 0)));\n            sw_poses2[0.4]->setValueTrans(Eigen::Vector3d(0, -6, 0));\n            mono_obs2[0.4] = calcMonoMeas(pf, sw_poses2.at(0.4));\n            stereo_obs2[0.4] = calcStereoMeas(pf, sw_poses2.at(0.4));\n            \n            sw_poses2[0.5] = std::make_shared<SE3>();\n            sw_poses2[0.5]->setValueLinearByQuat(Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d(0, 0, 1), Eigen::Vector3d(-1, 0, 0)));\n            sw_poses2[0.5]->setValueTrans(Eigen::Vector3d(5.5, 0, 0));\n            mono_obs2[0.5] = calcMonoMeas(pf, sw_poses2.at(0.5));\n            stereo_obs2[0.5] = calcStereoMeas(pf, sw_poses2.at(0.5));\n            \n            sw_poses2[0.6] = std::make_shared<SE3>();\n            sw_poses2[0.6]->setValueLinearByQuat(Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d(0, 0, 1), Eigen::Vector3d(1, 0, 0)));\n            sw_poses2[0.6]->setValueTrans(Eigen::Vector3d(-10, 0, 0));\n            mono_obs2[0.6] = calcMonoMeas(pf, sw_poses2.at(0.6));\n            stereo_obs2[0.6] = calcStereoMeas(pf, sw_poses2.at(0.6));\n        }\n        \n        virtual ~TestTriangulator() {}\n    protected:\n        Eigen::Vector3d pf;\n        \n        Eigen::Isometry3d T_cl2cr;\n        \n        std::map<double, std::shared_ptr<SE3>> sw_poses1, sw_poses2;\n        \n        std::map<double, std::shared_ptr<MonoMeas>> mono_obs1, mono_obs2;\n        \n        std::map<double, std::shared_ptr<StereoMeas>> stereo_obs1, stereo_obs2;\n        \n        std::shared_ptr<MonoMeas> calcMonoMeas(const Eigen::Vector3d& pf, const std::shared_ptr<SE3> pose)\n        {\n            std::shared_ptr<MonoMeas> mono_meas(new MonoMeas());\n            \n            Eigen::Vector3d body = pose->copyValueAsIso().inverse()*pf;\n            \n            mono_meas->_u0 = body.x()/body.z() + generateGaussRandom(0.0, 0.02);\n            mono_meas->_v0 = body.y()/body.z() + generateGaussRandom(0.0, 0.02);\n            \n            return mono_meas;\n        }\n        \n        std::shared_ptr<StereoMeas> calcStereoMeas(const Eigen::Vector3d& pf, const std::shared_ptr<SE3> pose)\n        {\n            std::shared_ptr<StereoMeas> stereo_meas(new StereoMeas());\n            \n            Eigen::Vector3d body_left = pose->copyValueAsIso().inverse()*pf;\n            Eigen::Vector3d body_right = T_cl2cr*pose->copyValueAsIso().inverse()*pf;\n            \n            stereo_meas->_u0 = body_left.x()/body_left.z() + generateGaussRandom(0.0, 0.02);\n            stereo_meas->_v0 = body_left.y()/body_left.z() + generateGaussRandom(0.0, 0.02);\n            \n            stereo_meas->_u1 = body_right.x()/body_right.z() + generateGaussRandom(0.0, 0.02);\n            stereo_meas->_v1 = body_right.y()/body_right.z() + generateGaussRandom(0.0, 0.02);\n            \n            // std::cout << \" u0 = \" << stereo_meas->_u0 << \" v0 = \" << stereo_meas->_v0 << \" u1 = \" << stereo_meas->_u1 << \" v1 = \" << stereo_meas->_v1 << std::endl;\n            \n            return stereo_meas;\n        }\n    };\n    \n    TEST_F(TestTriangulator, monoTriangulate)\n    {\n        std::shared_ptr<Triangulator> tri(new Triangulator());\n        \n        Eigen::Vector3d result;\n        \n        bool flag = tri->triangulateMonoObs(mono_obs1, sw_poses1, result);\n        \n        std::cout << \"pf ref = \" << pf.transpose() << std::endl;\n        std::cout << \"mono triangulation result = \" << result.transpose() << std::endl;\n        \n        ASSERT_NEAR((pf-result).norm(), 0.0, 0.05);\n        ASSERT_TRUE(flag);\n        \n        flag = tri->triangulateMonoObs(mono_obs2, sw_poses2, result);\n        \n        std::cout << \"pf ref = \" << pf.transpose() << std::endl;\n        std::cout << \"mono triangulation result = \" << result.transpose() << std::endl;\n        \n        ASSERT_NEAR((pf-result).norm(), 0.0, 0.15);\n        ASSERT_TRUE(flag);\n    }\n    \n    TEST_F(TestTriangulator, stereoTriangulate)\n    {\n        std::shared_ptr<Triangulator> tri(new Triangulator());\n        \n        Eigen::Vector3d result;\n        \n        bool flag = tri->triangulateStereoObs(stereo_obs1, sw_poses1, T_cl2cr, result);\n        \n        std::cout << \"pf ref = \" << pf.transpose() << std::endl;\n        std::cout << \"stereo triangulation result = \" << result.transpose() << std::endl;\n        \n        ASSERT_NEAR((pf-result).norm(), 0.0, 0.05);\n        ASSERT_TRUE(flag);\n        \n        flag = tri->triangulateStereoObs(stereo_obs2, sw_poses2, T_cl2cr, result);\n        \n        std::cout << \"pf ref = \" << pf.transpose() << std::endl;\n        std::cout << \"stereo triangulation result = \" << result.transpose() << std::endl;\n        \n        ASSERT_NEAR((pf-result).norm(), 0.0, 0.15);\n        ASSERT_TRUE(flag);\n    }\n    \n}\n\nint main(int argc, char** argv)\n{\n    testing::InitGoogleTest(&argc, argv);\n    return RUN_ALL_TESTS();\n}\n"
  }
]