[ { "path": ".gitignore", "content": "" }, { "path": "CMakeLists.txt", "content": "cmake_minimum_required(VERSION 2.8.3)\nproject(i2ekf_lo)\n\nSET(CMAKE_BUILD_TYPE \"Release\")\n\nADD_COMPILE_OPTIONS(-std=c++14 )\nADD_COMPILE_OPTIONS(-std=c++14 )\nset( CMAKE_CXX_FLAGS \"-std=c++14 -O3\" )\n\nadd_definitions(-DROOT_DIR=\\\"${CMAKE_CURRENT_SOURCE_DIR}/\\\")\n\nset(CMAKE_C_FLAGS \"${CMAKE_C_FLAGS} -fexceptions\" )\nset(CMAKE_CXX_STANDARD 14)\nset(CMAKE_CXX_STANDARD_REQUIRED ON)\nset(CMAKE_CXX_EXTENSIONS OFF)\nset(CMAKE_CXX_FLAGS \"${CMAKE_CXX_FLAGS} -std=c++14 -pthread -std=c++0x -std=c++14 -fexceptions\")\n\nmessage(\"Current CPU architecture: ${CMAKE_SYSTEM_PROCESSOR}\")\nif(CMAKE_SYSTEM_PROCESSOR MATCHES \"(x86)|(X86)|(amd64)|(AMD64)\" )\n include(ProcessorCount)\n ProcessorCount(N)\n message(\"Processer number: ${N}\")\n if(N GREATER 4)\n add_definitions(-DMP_EN)\n add_definitions(-DMP_PROC_NUM=3)\n message(\"core for MP: 3\")\n elseif(N GREATER 3)\n add_definitions(-DMP_EN)\n add_definitions(-DMP_PROC_NUM=2)\n message(\"core for MP: 2\")\n else()\n add_definitions(-DMP_PROC_NUM=1)\n endif()\nelse()\n add_definitions(-DMP_PROC_NUM=1)\nendif()\n\nadd_subdirectory(Thirdparty/Sophus)\nadd_subdirectory(Thirdparty/tessil-src)\n\nfind_package(OpenMP QUIET)\nset(CMAKE_CXX_FLAGS \"${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}\")\nset(CMAKE_C_FLAGS \"${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}\")\n\nfind_package(PythonLibs REQUIRED)\nfind_path(MATPLOTLIB_CPP_INCLUDE_DIRS \"matplotlibcpp.h\")\n\nfind_package(catkin REQUIRED COMPONENTS\n geometry_msgs\n nav_msgs\n sensor_msgs\n roscpp\n rospy\n std_msgs\n pcl_ros\n tf\n livox_ros_driver\n message_generation\n eigen_conversions\n)\n\nfind_package(Eigen3 REQUIRED)\nfind_package(PCL 1.8 REQUIRED)\nfind_package(Ceres REQUIRED)\n\nmessage(Eigen: ${EIGEN3_INCLUDE_DIR})\n\ninclude_directories(\n ../../devel/include\n\t${catkin_INCLUDE_DIRS} \n ${EIGEN3_INCLUDE_DIR}\n ${PCL_INCLUDE_DIRS}\n ${PYTHON_INCLUDE_DIRS}\n include)\n\nadd_message_files(\n FILES\n Pose6D.msg\n States.msg\n)\n\ngenerate_messages(\n DEPENDENCIES\n geometry_msgs\n)\n\ncatkin_package(\n CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs message_runtime\n DEPENDS EIGEN3 PCL\n INCLUDE_DIRS\n)\n\nadd_executable(i2ekf_lo_mapping\n src/laserMapping.cpp\n include/ikd-Tree/ikd_Tree.cpp\n src/preprocess.cpp)\n\nadd_dependencies(i2ekf_lo_mapping ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})\n\ntarget_link_libraries(i2ekf_lo_mapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES} ${CERES_LIBRARIES} Sophus::Sophus tsl::robin_map)\ntarget_include_directories(i2ekf_lo_mapping PRIVATE ${PYTHON_INCLUDE_DIRS})\n\n\n\n" }, { "path": "LICENSE", "content": " GNU GENERAL PUBLIC LICENSE\n Version 2, June 1991\n\n Copyright (C) 1989, 1991 Free Software Foundation, Inc.,\n 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA\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 licenses for most software are designed to take away your\nfreedom to share and change it. By contrast, the GNU General Public\nLicense is intended to guarantee your freedom to share and change free\nsoftware--to make sure the software is free for all its users. This\nGeneral Public License applies to most of the Free Software\nFoundation's software and to any other program whose authors commit to\nusing it. (Some other Free Software Foundation software is covered by\nthe GNU Lesser General Public License instead.) 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\nthis service if you wish), that you receive source code or can get it\nif you want it, that you can change the software or use pieces of it\nin new free programs; and that you know you can do these things.\n\n To protect your rights, we need to make restrictions that forbid\nanyone to deny you these rights or to ask you to surrender the rights.\nThese restrictions translate to certain responsibilities for you if you\ndistribute copies of the software, or if you modify it.\n\n For example, if you distribute copies of such a program, whether\ngratis or for a fee, you must give the recipients all the rights that\nyou have. You must make sure that they, too, receive or can get the\nsource code. And you must show them these terms so they know their\nrights.\n\n We protect your rights with two steps: (1) copyright the software, and\n(2) offer you this license which gives you legal permission to copy,\ndistribute and/or modify the software.\n\n Also, for each author's protection and ours, we want to make certain\nthat everyone understands that there is no warranty for this free\nsoftware. If the software is modified by someone else and passed on, we\nwant its recipients to know that what they have is not the original, so\nthat any problems introduced by others will not reflect on the original\nauthors' reputations.\n\n Finally, any free program is threatened constantly by software\npatents. We wish to avoid the danger that redistributors of a free\nprogram will individually obtain patent licenses, in effect making the\nprogram proprietary. To prevent this, we have made it clear that any\npatent must be licensed for everyone's free use or not licensed at all.\n\n The precise terms and conditions for copying, distribution and\nmodification follow.\n\n GNU GENERAL PUBLIC LICENSE\n TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION\n\n 0. This License applies to any program or other work which contains\na notice placed by the copyright holder saying it may be distributed\nunder the terms of this General Public License. The \"Program\", below,\nrefers to any such program or work, and a \"work based on the Program\"\nmeans either the Program or any derivative work under copyright law:\nthat is to say, a work containing the Program or a portion of it,\neither verbatim or with modifications and/or translated into another\nlanguage. (Hereinafter, translation is included without limitation in\nthe term \"modification\".) Each licensee is addressed as \"you\".\n\nActivities other than copying, distribution and modification are not\ncovered by this License; they are outside its scope. The act of\nrunning the Program is not restricted, and the output from the Program\nis covered only if its contents constitute a work based on the\nProgram (independent of having been made by running the Program).\nWhether that is true depends on what the Program does.\n\n 1. You may copy and distribute verbatim copies of the Program's\nsource code as you receive it, in any medium, provided that you\nconspicuously and appropriately publish on each copy an appropriate\ncopyright notice and disclaimer of warranty; keep intact all the\nnotices that refer to this License and to the absence of any warranty;\nand give any other recipients of the Program a copy of this License\nalong with the Program.\n\nYou may charge a fee for the physical act of transferring a copy, and\nyou may at your option offer warranty protection in exchange for a fee.\n\n 2. You may modify your copy or copies of the Program or any portion\nof it, thus forming a work based on the Program, and copy and\ndistribute such modifications or work under the terms of Section 1\nabove, provided that you also meet all of these conditions:\n\n a) You must cause the modified files to carry prominent notices\n stating that you changed the files and the date of any change.\n\n b) You must cause any work that you distribute or publish, that in\n whole or in part contains or is derived from the Program or any\n part thereof, to be licensed as a whole at no charge to all third\n parties under the terms of this License.\n\n c) If the modified program normally reads commands interactively\n when run, you must cause it, when started running for such\n interactive use in the most ordinary way, to print or display an\n announcement including an appropriate copyright notice and a\n notice that there is no warranty (or else, saying that you provide\n a warranty) and that users may redistribute the program under\n these conditions, and telling the user how to view a copy of this\n License. (Exception: if the Program itself is interactive but\n does not normally print such an announcement, your work based on\n the Program is not required to print an announcement.)\n\nThese requirements apply to the modified work as a whole. If\nidentifiable sections of that work are not derived from the Program,\nand can be reasonably considered independent and separate works in\nthemselves, then this License, and its terms, do not apply to those\nsections when you distribute them as separate works. But when you\ndistribute the same sections as part of a whole which is a work based\non the Program, the distribution of the whole must be on the terms of\nthis License, whose permissions for other licensees extend to the\nentire whole, and thus to each and every part regardless of who wrote it.\n\nThus, it is not the intent of this section to claim rights or contest\nyour rights to work written entirely by you; rather, the intent is to\nexercise the right to control the distribution of derivative or\ncollective works based on the Program.\n\nIn addition, mere aggregation of another work not based on the Program\nwith the Program (or with a work based on the Program) on a volume of\na storage or distribution medium does not bring the other work under\nthe scope of this License.\n\n 3. You may copy and distribute the Program (or a work based on it,\nunder Section 2) in object code or executable form under the terms of\nSections 1 and 2 above provided that you also do one of the following:\n\n a) Accompany it with the complete corresponding machine-readable\n source code, which must be distributed under the terms of Sections\n 1 and 2 above on a medium customarily used for software interchange; or,\n\n b) Accompany it with a written offer, valid for at least three\n years, to give any third party, for a charge no more than your\n cost of physically performing source distribution, a complete\n machine-readable copy of the corresponding source code, to be\n distributed under the terms of Sections 1 and 2 above on a medium\n customarily used for software interchange; or,\n\n c) Accompany it with the information you received as to the offer\n to distribute corresponding source code. (This alternative is\n allowed only for noncommercial distribution and only if you\n received the program in object code or executable form with such\n an offer, in accord with Subsection b above.)\n\nThe source code for a work means the preferred form of the work for\nmaking modifications to it. For an executable work, complete source\ncode means all the source code for all modules it contains, plus any\nassociated interface definition files, plus the scripts used to\ncontrol compilation and installation of the executable. However, as a\nspecial exception, the source code distributed need not include\nanything that is normally distributed (in either source or binary\nform) with the major components (compiler, kernel, and so on) of the\noperating system on which the executable runs, unless that component\nitself accompanies the executable.\n\nIf distribution of executable or object code is made by offering\naccess to copy from a designated place, then offering equivalent\naccess to copy the source code from the same place counts as\ndistribution of the source code, even though third parties are not\ncompelled to copy the source along with the object code.\n\n 4. You may not copy, modify, sublicense, or distribute the Program\nexcept as expressly provided under this License. Any attempt\notherwise to copy, modify, sublicense or distribute the Program is\nvoid, and will automatically terminate your rights under this License.\nHowever, parties who have received copies, or rights, from you under\nthis License will not have their licenses terminated so long as such\nparties remain in full compliance.\n\n 5. You are not required to accept this License, since you have not\nsigned it. However, nothing else grants you permission to modify or\ndistribute the Program or its derivative works. These actions are\nprohibited by law if you do not accept this License. Therefore, by\nmodifying or distributing the Program (or any work based on the\nProgram), you indicate your acceptance of this License to do so, and\nall its terms and conditions for copying, distributing or modifying\nthe Program or works based on it.\n\n 6. Each time you redistribute the Program (or any work based on the\nProgram), the recipient automatically receives a license from the\noriginal licensor to copy, distribute or modify the Program subject to\nthese terms and conditions. You may not impose any further\nrestrictions on the recipients' exercise of the rights granted herein.\nYou are not responsible for enforcing compliance by third parties to\nthis License.\n\n 7. If, as a consequence of a court judgment or allegation of patent\ninfringement or for any other reason (not limited to patent issues),\nconditions 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\ndistribute so as to satisfy simultaneously your obligations under this\nLicense and any other pertinent obligations, then as a consequence you\nmay not distribute the Program at all. For example, if a patent\nlicense would not permit royalty-free redistribution of the Program by\nall those who receive copies directly or indirectly through you, then\nthe only way you could satisfy both it and this License would be to\nrefrain entirely from distribution of the Program.\n\nIf any portion of this section is held invalid or unenforceable under\nany particular circumstance, the balance of the section is intended to\napply and the section as a whole is intended to apply in other\ncircumstances.\n\nIt is not the purpose of this section to induce you to infringe any\npatents or other property right claims or to contest validity of any\nsuch claims; this section has the sole purpose of protecting the\nintegrity of the free software distribution system, which is\nimplemented by public license practices. Many people have made\ngenerous contributions to the wide range of software distributed\nthrough that system in reliance on consistent application of that\nsystem; it is up to the author/donor to decide if he or she is willing\nto distribute software through any other system and a licensee cannot\nimpose that choice.\n\nThis section is intended to make thoroughly clear what is believed to\nbe a consequence of the rest of this License.\n\n 8. If the distribution and/or use of the Program is restricted in\ncertain countries either by patents or by copyrighted interfaces, the\noriginal copyright holder who places the Program under this License\nmay add an explicit geographical distribution limitation excluding\nthose countries, so that distribution is permitted only in or among\ncountries not thus excluded. In such case, this License incorporates\nthe limitation as if written in the body of this License.\n\n 9. The Free Software Foundation may publish revised and/or new versions\nof the 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\nEach version is given a distinguishing version number. If the Program\nspecifies a version number of this License which applies to it and \"any\nlater version\", you have the option of following the terms and conditions\neither of that version or of any later version published by the Free\nSoftware Foundation. If the Program does not specify a version number of\nthis License, you may choose any version ever published by the Free Software\nFoundation.\n\n 10. If you wish to incorporate parts of the Program into other free\nprograms whose distribution conditions are different, write to the author\nto ask for permission. For software which is copyrighted by the Free\nSoftware Foundation, write to the Free Software Foundation; we sometimes\nmake exceptions for this. Our decision will be guided by the two goals\nof preserving the free status of all derivatives of our free software and\nof promoting the sharing and reuse of software generally.\n\n NO WARRANTY\n\n 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY\nFOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN\nOTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES\nPROVIDE THE PROGRAM \"AS IS\" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED\nOR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF\nMERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS\nTO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE\nPROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING,\nREPAIR OR CORRECTION.\n\n 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING\nWILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR\nREDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES,\nINCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING\nOUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED\nTO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY\nYOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER\nPROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE\nPOSSIBILITY OF SUCH DAMAGES.\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\nconvey 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 \n Copyright (C) \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 2 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 along\n with this program; if not, write to the Free Software Foundation, Inc.,\n 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.\n\nAlso add information on how to contact you by electronic and paper mail.\n\nIf the program is interactive, make it output a short notice like this\nwhen it starts in an interactive mode:\n\n Gnomovision version 69, Copyright (C) year name of author\n Gnomovision 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, the commands you use may\nbe called something other than `show w' and `show c'; they could even be\nmouse-clicks or menu items--whatever suits your program.\n\nYou should also get your employer (if you work as a programmer) or your\nschool, if any, to sign a \"copyright disclaimer\" for the program, if\nnecessary. Here is a sample; alter the names:\n\n Yoyodyne, Inc., hereby disclaims all copyright interest in the program\n `Gnomovision' (which makes passes at compilers) written by James Hacker.\n\n , 1 April 1989\n Ty Coon, President of Vice\n\nThis General Public License does not permit incorporating your program into\nproprietary programs. If your program is a subroutine library, you may\nconsider it more useful to permit linking proprietary applications with the\nlibrary. If this is what you want to do, use the GNU Lesser General\nPublic License instead of this License.\n" }, { "path": "README.md", "content": "
\n

I2EKF-LO

\n A Dual-Iteration Extended Kalman Filter Based LiDAR Odometry\n
\n IROS 2024 Oral\n
\n \"Pipeline\n
\n
\n\n### Introduction\nLiDAR odometry is a pivotal technology in the fields of autonomous driving and autonomous mobile robotics. However, most of the current works focuse on nonlinear optimization methods, and there are still many challenges in using the traditional Iterative Extended Kalman Filter (IEKF) framework to tackle the problem: IEKF only iterates over the observation equation, relying on a rough estimate of the initial state, which is insufficient to fully eliminate motion distortion in the input point cloud; the system process noise is difficult to be determined during state estimation of the complex motions; and the varying motion models across different sensor carriers. To address these issues, we propose the Dual-Iteration Extended Kalman Filter (I2EKF) and the LiDAR odometry based on I2EKF (I2EKF-LO). This approach not only iterates over the observation equation but also leverages state updates to iteratively mitigate motion distortion in LiDAR point clouds. Moreover, it dynamically adjusts process noise based on the confidence level of prior predictions during state estimation and establishes motion models for different sensor carriers to achieve accurate and efficient state estimation. Comprehensive experiments demonstrate that I2EKF-LO achieves outstanding levels of accuracy and computational efficiency in the realm of LiDAR odometry.\n\n**Developers**: The codes of this repo are contributed by [Wenlu Yu (于文录)](https://github.com/YWL0720), [Jie Xu (徐杰)](https://github.com/jiejie567), [Chengwei Zhao (赵成伟)](https://github.com/chengwei0427)\n\n### News\n* **[30/06/2024]**: I2EKF-LO is accepted to IROS 2024.\n* **[01/07/2024]**: We are currently working on organizing and refining the complete code. The full version will be released soon.\n* **[02/07/2024]**: Updated the video link and submitted the paper to arxiv.\n\n### Related Paper\n\nRelated paper available on arxiv: [I2EKF-LO: A Dual-Iteration Extended Kalman Filter Based LiDAR Odometry](https://arxiv.org/abs/2407.02190) \n\n### Related Video:\n\n
\n \n \n
\n\n## 1. Prerequisites\n\n### 1.1 **Ubuntu** and **ROS**\n\nUbuntu >= 18.04.\n\nROS >= Melodic. [ROS Installation](http://wiki.ros.org/ROS/Installation)\n\n### 1.2. **PCL && Eigen**\n\nPCL >= 1.8, Follow [PCL Installation](http://www.pointclouds.org/downloads/linux.html).\n\nEigen >= 3.3.4, Follow [Eigen Installation](http://eigen.tuxfamily.org/index.php?title=Main_Page).\n\n### 1.3. **livox_ros_driver**\n\nFollow [livox_ros_driver Installation](https://github.com/Livox-SDK/livox_ros_driver).\n\n\n## 2. Build\n\nClone the repository and catkin_make:\n\n```\ncd ~/catkin_ws/src\ngit clone https://github.com/YWL0720/I2EKF-LO\ncd ..\ncatkin_make -j\nsource devel/setup.bash\n```\n\n## 3. Directly run\n\n```bash\ncd ~/$I2EKF_LO_ROS_DIR$\nsource devel/setup.bash\nroslaunch i2ekf_lo xxx.launch\n```\n## 4. Rosbag Example\nDownload our test bags here: [HIT-TIB Datasets](https://drive.google.com/drive/folders/1L5cX9uyAiZei17oq7ELyd8WyIRReHq8u?usp=drive_link).\n\n## 5. Acknowledgments\n\nThanks for [Fast-LIO2](https://github.com/hku-mars/FAST_LIO) (Fast Direct LiDAR-inertial Odometry) and [LI-INIT](https://github.com/hku-mars/LiDAR_IMU_Init)(Robust Real-time LiDAR-inertial Initialization).\n\n## 6. License\n\nThe source code is released under [GPLv2](http://www.gnu.org/licenses/) license.\n\n## 7. TODO(s)\n- [ ] Updated video link\n- [ ] Upload a preprint of our paper\n" }, { "path": "Thirdparty/Sophus/.clang-format", "content": "Language: Cpp\n# BasedOnStyle: Google\nAccessModifierOffset: -1\nAlignAfterOpenBracket: Align\nAlignConsecutiveAssignments: false\nAlignConsecutiveDeclarations: false\nAlignEscapedNewlinesLeft: true\nAlignOperands: true\nAlignTrailingComments: true\nAllowAllParametersOfDeclarationOnNextLine: true\nAllowShortBlocksOnASingleLine: false\nAllowShortCaseLabelsOnASingleLine: false\nAllowShortFunctionsOnASingleLine: All\nAllowShortIfStatementsOnASingleLine: true\nAllowShortLoopsOnASingleLine: true\nAlwaysBreakAfterDefinitionReturnType: None\nAlwaysBreakAfterReturnType: None\nAlwaysBreakBeforeMultilineStrings: true\nAlwaysBreakTemplateDeclarations: true\nBinPackArguments: true\nBinPackParameters: true\nBraceWrapping:\n AfterClass: false\n AfterControlStatement: false\n AfterEnum: false\n AfterFunction: false\n AfterNamespace: false\n AfterObjCDeclaration: false\n AfterStruct: false\n AfterUnion: false\n BeforeCatch: false\n BeforeElse: false\n IndentBraces: false\nBreakBeforeBinaryOperators: None\nBreakBeforeBraces: Attach\nBreakBeforeTernaryOperators: true\nBreakConstructorInitializersBeforeComma: false\nColumnLimit: 80\nCommentPragmas: '^ IWYU pragma:'\nConstructorInitializerAllOnOneLineOrOnePerLine: true\nConstructorInitializerIndentWidth: 4\nContinuationIndentWidth: 4\nCpp11BracedListStyle: true\nDerivePointerAlignment: true\nDisableFormat: false\nExperimentalAutoDetectBinPacking: false\nForEachMacros: [ foreach, Q_FOREACH, BOOST_FOREACH ]\nIncludeCategories:\n - Regex: '^<.*\\.h>'\n Priority: 1\n - Regex: '^<.*'\n Priority: 2\n - Regex: '.*'\n Priority: 3\nIndentCaseLabels: true\nIndentWidth: 2\nIndentWrappedFunctionNames: false\nKeepEmptyLinesAtTheStartOfBlocks: false\nMacroBlockBegin: ''\nMacroBlockEnd: ''\nMaxEmptyLinesToKeep: 1\nNamespaceIndentation: None\nObjCBlockIndentWidth: 2\nObjCSpaceAfterProperty: false\nObjCSpaceBeforeProtocolList: false\nPenaltyBreakBeforeFirstCallParameter: 1\nPenaltyBreakComment: 300\nPenaltyBreakFirstLessLess: 120\nPenaltyBreakString: 1000\nPenaltyExcessCharacter: 1000000\nPenaltyReturnTypeOnItsOwnLine: 200\nPointerAlignment: Left\nReflowComments: true\nSortIncludes: true\nSpaceAfterCStyleCast: false\nSpaceBeforeAssignmentOperators: true\nSpaceBeforeParens: ControlStatements\nSpaceInEmptyParentheses: false\nSpacesBeforeTrailingComments: 2\nSpacesInAngles: false\nSpacesInContainerLiterals: true\nSpacesInCStyleCastParentheses: false\nSpacesInParentheses: false\nSpacesInSquareBrackets: false\nStandard: Auto\nTabWidth: 8\nUseTab: Never\n" }, { "path": "Thirdparty/Sophus/CMakeLists.txt", "content": "cmake_minimum_required(VERSION 3.4)\nproject(Sophus VERSION 1.22.10)\n\ninclude(CMakePackageConfigHelpers)\ninclude(GNUInstallDirs)\n\n# Determine if sophus is built as a subproject (using add_subdirectory)\n# or if it is the master project.\nif (NOT DEFINED SOPHUS_MASTER_PROJECT)\n set(SOPHUS_MASTER_PROJECT OFF)\n if (CMAKE_CURRENT_SOURCE_DIR STREQUAL CMAKE_SOURCE_DIR)\n set(SOPHUS_MASTER_PROJECT ON)\n message(STATUS \"CMake version: ${CMAKE_VERSION}\")\n endif ()\nendif ()\n\noption(SOPHUS_INSTALL \"Generate the install target.\" ${SOPHUS_MASTER_PROJECT})\noption(SOPHUS_USE_BASIC_LOGGING \"Use basic logging (in ensure and test macros)\" OFF)\n\nif(SOPHUS_MASTER_PROJECT)\n # Release by default\n # Turn on Debug with \"-DCMAKE_BUILD_TYPE=Debug\"\n if(NOT CMAKE_BUILD_TYPE)\n set(CMAKE_BUILD_TYPE Release)\n endif()\n\n set(CMAKE_CXX_STANDARD 14)\n\n # Set compiler specific settings (FixMe: Should not cmake do this for us automatically?)\n IF(\"${CMAKE_CXX_COMPILER_ID}\" STREQUAL \"Clang\")\n SET(CMAKE_CXX_FLAGS_DEBUG \"-O0 -g\")\n SET(CMAKE_CXX_FLAGS_RELEASE \"-O3\")\n SET(CMAKE_CXX_FLAGS \"${CMAKE_CXX_FLAGS} -Wall -Werror -Wextra -Wno-deprecated-register -Qunused-arguments -fcolor-diagnostics\")\n ELSEIF(\"${CMAKE_CXX_COMPILER_ID}\" STREQUAL \"GNU\")\n SET(CMAKE_CXX_FLAGS_DEBUG \"-O0 -g\")\n SET(CMAKE_CXX_FLAGS_RELEASE \"-O3\")\n SET(CMAKE_CXX_FLAGS \"${CMAKE_CXX_FLAGS} -Wall -Werror -Wextra -std=c++14 -Wno-deprecated-declarations -ftemplate-backtrace-limit=0\")\n SET(CMAKE_CXX_FLAGS_COVERAGE \"${CMAKE_CXX_FLAGS_DEBUG} --coverage -fno-inline -fno-inline-small-functions -fno-default-inline\")\n SET(CMAKE_EXE_LINKER_FLAGS_COVERAGE \"${CMAKE_EXE_LINKER_FLAGS_DEBUG} --coverage\")\n SET(CMAKE_SHARED_LINKER_FLAGS_COVERAGE \"${CMAKE_SHARED_LINKER_FLAGS_DEBUG} --coverage\")\n ELSEIF(CMAKE_CXX_COMPILER_ID MATCHES \"^MSVC$\")\n ADD_DEFINITIONS(\"-D _USE_MATH_DEFINES /bigobj /wd4305 /wd4244 /MP\")\n ENDIF()\n\n # Add local path for finding packages, set the local version first\n list(APPEND CMAKE_MODULE_PATH \"${CMAKE_CURRENT_SOURCE_DIR}/cmake_modules\")\nendif()\n\nif(SOPHUS_USE_BASIC_LOGGING)\n set (CMAKE_DISABLE_FIND_PACKAGE_fmt ON)\nendif()\n\n# Find public dependencies if targets are not yet defined. (Targets might be for example\n# defined by a parent project including Sophus via `add_subdirectory`.)\n\nif(NOT TARGET Eigen3::Eigen)\n find_package(Eigen3 3.3.0 REQUIRED)\nendif()\n\nif(NOT TARGET fmt::fmt)\n find_package(fmt)\nendif()\n\n\n# Define interface library target\nadd_library(sophus INTERFACE)\nadd_library (Sophus::Sophus ALIAS sophus)\n\nset(SOPHUS_HEADER_FILES\n sophus/average.hpp\n sophus/cartesian.hpp\n sophus/ceres_local_parameterization.hpp\n sophus/ceres_manifold.hpp\n sophus/ceres_typetraits.hpp\n sophus/common.hpp\n sophus/geometry.hpp\n sophus/interpolate.hpp\n sophus/interpolate_details.hpp\n sophus/num_diff.hpp\n sophus/rotation_matrix.hpp\n sophus/rxso2.hpp\n sophus/rxso3.hpp\n sophus/se2.hpp\n sophus/se3.hpp\n sophus/sim2.hpp\n sophus/sim3.hpp\n sophus/sim_details.hpp\n sophus/so2.hpp\n sophus/so3.hpp\n sophus/spline.hpp\n sophus/types.hpp\n sophus/velocities.hpp\n)\n\nset(SOPHUS_OTHER_FILES\n sophus/test_macros.hpp\n sophus/example_ensure_handler.cpp\n)\n\nif(MSVC)\n # Define common math constants if we compile with MSVC\n target_compile_definitions (sophus INTERFACE _USE_MATH_DEFINES)\nendif (MSVC)\n\n# Add Eigen interface dependency, depending on available cmake info\nif(TARGET Eigen3::Eigen)\n target_link_libraries(sophus INTERFACE Eigen3::Eigen)\n set(Eigen3_DEPENDENCY \"find_dependency (Eigen3 ${Eigen3_VERSION})\")\nelse()\n target_include_directories (sophus SYSTEM INTERFACE ${EIGEN3_INCLUDE_DIR})\nendif()\n\nif(SOPHUS_USE_BASIC_LOGGING OR NOT TARGET fmt::fmt)\n # NOTE fmt_FOUND does not seem to be defined even though the package config\n # was found.\n target_compile_definitions(sophus INTERFACE SOPHUS_USE_BASIC_LOGGING=1)\n message(STATUS \"Turning basic logging ON\")\nelse()\n target_link_libraries(sophus INTERFACE fmt::fmt)\n set(fmt_DEPENDENCY \"find_dependency (fmt ${fmt_VERSION})\")\n message(STATUS \"Turning basic logging OFF\")\nendif()\n\n# Associate target with include directory\ntarget_include_directories(sophus INTERFACE\n \"$\"\n \"$\"\n)\n\n# Declare all used C++14 features\ntarget_compile_features (sophus INTERFACE\n cxx_auto_type\n cxx_decltype\n cxx_nullptr\n cxx_right_angle_brackets\n cxx_variadic_macros\n cxx_variadic_templates\n)\n\n# Add sources as custom target so that they are shown in IDE's\nadd_custom_target(other SOURCES ${SOPHUS_OTHER_FILES} ${SOPHUS_HEADER_FILES})\n\n# Create 'test' make target using ctest\noption(BUILD_SOPHUS_TESTS \"Build tests.\" ON)\nif(BUILD_SOPHUS_TESTS)\n enable_testing()\n add_subdirectory(test)\nendif()\n\n# Create examples make targets using ctest\noption(BUILD_SOPHUS_EXAMPLES \"Build examples.\" ON)\nif(BUILD_SOPHUS_EXAMPLES)\n add_subdirectory(examples)\nendif()\n\n# Build python sophus bindings\noption(BUILD_PYTHON_BINDINGS \"Build python sophus bindings.\" OFF)\nif(BUILD_PYTHON_BINDINGS)\n include(FetchContent)\n FetchContent_Declare(\n pybind11\n GIT_REPOSITORY https://github.com/pybind/pybind11.git\n GIT_TAG master\n )\n FetchContent_MakeAvailable(pybind11)\n\n add_subdirectory(${pybind11_SOURCE_DIR} ${CMAKE_CURRENT_BINARY_DIR}/pybind)\n pybind11_add_module(sophus_pybind ${CMAKE_CURRENT_SOURCE_DIR}/sophus_pybind/bindings.cpp)\n target_link_libraries(sophus_pybind PUBLIC sophus)\nendif(BUILD_PYTHON_BINDINGS)\n\nif(SOPHUS_INSTALL)\n # Export package for use from the build tree\n set(SOPHUS_CMAKE_EXPORT_DIR ${CMAKE_INSTALL_DATADIR}/sophus/cmake)\n\n set_target_properties(sophus PROPERTIES EXPORT_NAME Sophus)\n\n install(TARGETS sophus EXPORT SophusTargets)\n install(EXPORT SophusTargets\n NAMESPACE Sophus::\n DESTINATION ${SOPHUS_CMAKE_EXPORT_DIR}\n )\n\n export(TARGETS sophus NAMESPACE Sophus:: FILE SophusTargets.cmake)\n export(PACKAGE Sophus)\n\n configure_package_config_file(\n SophusConfig.cmake.in\n ${CMAKE_CURRENT_BINARY_DIR}/SophusConfig.cmake\n INSTALL_DESTINATION ${SOPHUS_CMAKE_EXPORT_DIR}\n NO_CHECK_REQUIRED_COMPONENTS_MACRO\n )\n\n # Remove architecture dependence. Sophus is a header-only library.\n set(TEMP_SIZEOF_VOID_P ${CMAKE_SIZEOF_VOID_P})\n unset(CMAKE_SIZEOF_VOID_P)\n\n # Write version to file\n write_basic_package_version_file (\n SophusConfigVersion.cmake\n VERSION ${PROJECT_VERSION}\n COMPATIBILITY SameMajorVersion\n )\n\n # Recover architecture dependence\n set(CMAKE_SIZEOF_VOID_P ${TEMP_SIZEOF_VOID_P})\n\n # Install cmake targets\n install(\n FILES ${CMAKE_CURRENT_BINARY_DIR}/SophusConfig.cmake\n ${CMAKE_CURRENT_BINARY_DIR}/SophusConfigVersion.cmake\n DESTINATION ${SOPHUS_CMAKE_EXPORT_DIR}\n )\n\n # Install header files\n install(\n FILES ${SOPHUS_HEADER_FILES}\n DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sophus\n )\nendif()\n" }, { "path": "Thirdparty/Sophus/LICENSE.txt", "content": "Copyright 2011-2017 Hauke Strasdat \n 2012-2017 Steven Lovegrove\n\nPermission is hereby granted, free of charge, to any person obtaining a copy\nof this software and associated documentation files (the \"Software\"), to\ndeal in the Software without restriction, including without limitation the\nrights to use, copy, modify, merge, publish, distribute, sublicense, and/or\nsell copies of the Software, and to permit persons to whom the Software is\nfurnished to do so, subject to the following conditions:\n\nThe above copyright notice and this permission notice shall be included in\nall copies or substantial portions of the Software.\n\nTHE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\nIMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\nFITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE\nAUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\nLIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING\nFROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS\nIN THE SOFTWARE." }, { "path": "Thirdparty/Sophus/README.rst", "content": "|GithubCICpp|_ windows: |AppVeyor|_ |GithubCISympy|_ |ci_cov|_\n\n\nSophus\n======\n\nOverview\n--------\n\nThis is a c++ implementation of Lie groups commonly used for 2d and 3d\ngeometric problems (i.e. for Computer Vision or Robotics applications).\nAmong others, this package includes the special orthogonal groups SO(2) and\nSO(3) to present rotations in 2d and 3d as well as the special Euclidean group\nSE(2) and SE(3) to represent rigid body transformations (i.e. rotations and\ntranslations) in 2d and 3d.\n\nAPI documentation: https://strasdat.github.io/Sophus/\n\nCross platform support\n----------------------\n\nSophus compiles with clang and gcc on Linux and OS X as well as msvc on Windows.\nThe specific compiler and operating system versions which are supported are\nthe ones which are used in the Continuous Integration (CI): See GitHubCI_ and\nAppVeyor_ for details.\n\nHowever, it should work (with no to minor modification) on many other\nmodern configurations as long they support c++14, CMake, Eigen 3.3.X and\n(optionally) fmt. The fmt dependency can be eliminated by passing\n\"-DUSE_BASIC_LOGGING=ON\" to cmake when configuring Sophus.\n\n.. _GitHubCI: https://github.com/strasdat/Sophus/actions\n\n.. |AppVeyor| image:: https://ci.appveyor.com/api/projects/status/um4285lwhs8ci7pt/branch/master?svg=true\n.. _AppVeyor: https://ci.appveyor.com/project/strasdat/sophus/branch/master\n\n.. |ci_cov| image:: https://coveralls.io/repos/github/strasdat/Sophus/badge.svg?branch=master\n.. _ci_cov: https://coveralls.io/github/strasdat/Sophus?branch=master\n\n.. |GithubCICpp| image:: https://github.com/strasdat/Sophus/actions/workflows/main.yml/badge.svg?branch=master\n.. _GithubCICpp: https://github.com/strasdat/Sophus/actions/workflows/main.yml?query=branch%3Amaster\n\n.. |GithubCISympy| image:: https://github.com/strasdat/Sophus/actions/workflows/sympy.yml/badge.svg?branch=master\n.. _GithubCISympy: https://github.com/strasdat/Sophus/actions/workflows/sympy.yml?query=branch%3Amaster\n" }, { "path": "Thirdparty/Sophus/Sophus.code-workspace", "content": "{\n\t\"folders\": [\n\t\t{\n\t\t\t\"path\": \".\"\n\t\t}\n\t],\n\t\"settings\": {}\n}" }, { "path": "Thirdparty/Sophus/SophusConfig.cmake.in", "content": "@PACKAGE_INIT@\n\ninclude (CMakeFindDependencyMacro)\n\n@Eigen3_DEPENDENCY@\n@fmt_DEPENDENCY@\n\ninclude (\"${CMAKE_CURRENT_LIST_DIR}/SophusTargets.cmake\")\n" }, { "path": "Thirdparty/Sophus/appveyor.yml", "content": "branches:\n only:\n - master\n\nos: Visual Studio 2015\n\nclone_folder: c:\\projects\\sophus\n\nplatform: x64\nconfiguration: Debug\n\nbuild:\n project: c:\\projects\\sophus\\build\\Sophus.sln\n\ninstall:\n - ps: wget https://gitlab.com/libeigen/eigen/-/archive/3.3.4/eigen-3.3.4.zip -outfile eigen3.zip\n - cmd: 7z x eigen3.zip -o\"C:\\projects\" -y > nul\n - git clone https://github.com/fmtlib/fmt.git\n - cd fmt\n - git checkout 5.3.0\n - mkdir build\n - cd build\n - cmake -G \"Visual Studio 14 2015 Win64\" -DCMAKE_BUILD_TYPE=Debug ..\n - cmake --build .\n - cmake --build . --target install\n - cd ../..\n\nbefore_build:\n - cd c:\\projects\\sophus\n - mkdir build\n - cd build\n - cmake -G \"Visual Studio 14 2015 Win64\" -DCMAKE_BUILD_TYPE=Debug -D EIGEN3_INCLUDE_DIR=C:\\projects\\eigen-3.3.4 ..\n\nafter_build:\n - ctest --output-on-failure\n" }, { "path": "Thirdparty/Sophus/cmake_modules/FindEigen3.cmake", "content": "# - Try to find Eigen3 lib\n#\n# This module supports requiring a minimum version, e.g. you can do\n# find_package(Eigen3 3.1.2)\n# to require version 3.1.2 or newer of Eigen3.\n#\n# Once done this will define\n#\n# EIGEN3_FOUND - system has eigen lib with correct version\n# EIGEN3_INCLUDE_DIR - the eigen include directory\n# EIGEN3_VERSION - eigen version\n#\n# and the following imported target:\n#\n# Eigen3::Eigen - The header-only Eigen library\n#\n# This module reads hints about search locations from \n# the following environment variables:\n#\n# EIGEN3_ROOT\n# EIGEN3_ROOT_DIR\n\n# Copyright (c) 2006, 2007 Montel Laurent, \n# Copyright (c) 2008, 2009 Gael Guennebaud, \n# Copyright (c) 2009 Benoit Jacob \n# Redistribution and use is allowed according to the terms of the 2-clause BSD license.\n\nif(NOT Eigen3_FIND_VERSION)\n if(NOT Eigen3_FIND_VERSION_MAJOR)\n set(Eigen3_FIND_VERSION_MAJOR 2)\n endif()\n if(NOT Eigen3_FIND_VERSION_MINOR)\n set(Eigen3_FIND_VERSION_MINOR 91)\n endif()\n if(NOT Eigen3_FIND_VERSION_PATCH)\n set(Eigen3_FIND_VERSION_PATCH 0)\n endif()\n\n set(Eigen3_FIND_VERSION \"${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}\")\nendif()\n\nmacro(_eigen3_check_version)\n file(READ \"${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h\" _eigen3_version_header)\n\n string(REGEX MATCH \"define[ \\t]+EIGEN_WORLD_VERSION[ \\t]+([0-9]+)\" _eigen3_world_version_match \"${_eigen3_version_header}\")\n set(EIGEN3_WORLD_VERSION \"${CMAKE_MATCH_1}\")\n string(REGEX MATCH \"define[ \\t]+EIGEN_MAJOR_VERSION[ \\t]+([0-9]+)\" _eigen3_major_version_match \"${_eigen3_version_header}\")\n set(EIGEN3_MAJOR_VERSION \"${CMAKE_MATCH_1}\")\n string(REGEX MATCH \"define[ \\t]+EIGEN_MINOR_VERSION[ \\t]+([0-9]+)\" _eigen3_minor_version_match \"${_eigen3_version_header}\")\n set(EIGEN3_MINOR_VERSION \"${CMAKE_MATCH_1}\")\n\n set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION})\n if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})\n set(EIGEN3_VERSION_OK FALSE)\n else()\n set(EIGEN3_VERSION_OK TRUE)\n endif()\n\n if(NOT EIGEN3_VERSION_OK)\n\n message(STATUS \"Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, \"\n \"but at least version ${Eigen3_FIND_VERSION} is required\")\n endif()\nendmacro()\n\nif (EIGEN3_INCLUDE_DIR)\n\n # in cache already\n _eigen3_check_version()\n set(EIGEN3_FOUND ${EIGEN3_VERSION_OK})\n set(Eigen3_FOUND ${EIGEN3_VERSION_OK})\n\nelse ()\n \n # search first if an Eigen3Config.cmake is available in the system,\n # if successful this would set EIGEN3_INCLUDE_DIR and the rest of\n # the script will work as usual\n find_package(Eigen3 ${Eigen3_FIND_VERSION} NO_MODULE QUIET)\n\n if(NOT EIGEN3_INCLUDE_DIR)\n find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library\n HINTS\n ENV EIGEN3_ROOT \n ENV EIGEN3_ROOT_DIR\n PATHS\n ${CMAKE_INSTALL_PREFIX}/include\n ${KDE4_INCLUDE_DIR}\n PATH_SUFFIXES eigen3 eigen\n )\n endif()\n\n if(EIGEN3_INCLUDE_DIR)\n _eigen3_check_version()\n endif()\n\n include(FindPackageHandleStandardArgs)\n find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK)\n\n mark_as_advanced(EIGEN3_INCLUDE_DIR)\n\nendif()\n\nif(EIGEN3_FOUND AND NOT TARGET Eigen3::Eigen)\n add_library(Eigen3::Eigen INTERFACE IMPORTED)\n set_target_properties(Eigen3::Eigen PROPERTIES\n INTERFACE_INCLUDE_DIRECTORIES \"${EIGEN3_INCLUDE_DIR}\")\nendif()\n" }, { "path": "Thirdparty/Sophus/doxyfile", "content": "DOXYFILE_ENCODING = UTF-8\nPROJECT_NAME = \"Sophus\"\nINPUT = sophus\nEXTRACT_ALL = YES\nENABLE_PREPROCESSING = YES\nMACRO_EXPANSION = YES\nWARN_AS_ERROR = YES\nEXPAND_ONLY_PREDEF = NO\nSKIP_FUNCTION_MACROS = NO\nAUTOLINK_SUPPORT = YES\nMULTILINE_CPP_IS_BRIEF = YES\nMARKDOWN_SUPPORT = YES\nINLINE_INHERITED_MEMB = NO\nEXCLUDE_SYMBOLS = Eigen::internal Sophus::details Sophus::interp_details Sophus::experimental\nGENERATE_LATEX = NO\nSTRIP_CODE_COMMENTS = NO\n\nGENERATE_XML = YES\nGENERATE_HTML = NO\nXML_OUTPUT = xml-dir\nXML_PROGRAMLISTING = NO\nCASE_SENSE_NAMES = NO\nHIDE_UNDOC_RELATIONS = YES\nEXTRACT_ALL = YES" }, { "path": "Thirdparty/Sophus/doxyrest-config.lua", "content": "FRAME_DIR_LIST = { \"doxyrest_b/doxyrest/frame/cfamily\", \"doxyrest_b/doxyrest/frame/common\" }\nFRAME_FILE = \"index.rst.in\"\nINPUT_FILE = \"xml-dir/index.xml\"\nOUTPUT_FILE = \"rst-dir/index.rst\"\nINTRO_FILE = \"page_index.rst\"\nSORT_GROUPS_BY = \"title\"\nGLOBAL_AUX_COMPOUND_ID = \"group_global\"\nLANGUAGE = cpp\nVERBATIM_TO_CODE_BLOCK = \"cpp\"\nESCAPE_ASTERISKS = true\nESCAPE_PIPES = true\nESCAPE_TRAILING_UNDERSCORES = true\nPROTECTION_FILTER = \"protected\"\nEXCLUDE_EMPTY_DEFINES = true\nEXCLUDE_DEFAULT_CONSTRUCTORS = false\nEXCLUDE_DESTRUCTORS = false\nEXCLUDE_PRIMITIVE_TYPEDEFS = true\nSHOW_DIRECT_DESCENDANTS = true\nTYPEDEF_TO_USING = true\nML_PARAM_LIST_LENGTH_THRESHOLD = 80" }, { "path": "Thirdparty/Sophus/examples/CMakeLists.txt", "content": "# Tests to run\nSET( EXAMPLE_SOURCES HelloSO3)\n\nFOREACH(example_src ${EXAMPLE_SOURCES})\n ADD_EXECUTABLE( ${example_src} ${example_src}.cpp)\n TARGET_LINK_LIBRARIES( ${example_src} sophus)\nENDFOREACH(example_src)\n" }, { "path": "Thirdparty/Sophus/examples/HelloSO3.cpp", "content": "#include \n#include \"sophus/geometry.hpp\"\n\nint main() {\n // The following demonstrates the group multiplication of rotation matrices\n\n // Create rotation matrices from rotations around the x and y and z axes:\n const double kPi = Sophus::Constants::pi();\n Sophus::SO3d R1 = Sophus::SO3d::rotX(kPi / 4);\n Sophus::SO3d R2 = Sophus::SO3d::rotY(kPi / 6);\n Sophus::SO3d R3 = Sophus::SO3d::rotZ(-kPi / 3);\n\n std::cout << \"The rotation matrices are\" << std::endl;\n std::cout << \"R1:\\n\" << R1.matrix() << std::endl;\n std::cout << \"R2:\\n\" << R2.matrix() << std::endl;\n std::cout << \"R3:\\n\" << R3.matrix() << std::endl;\n std::cout << \"Their product R1*R2*R3:\\n\"\n << (R1 * R2 * R3).matrix() << std::endl;\n std::cout << std::endl;\n\n // Rotation matrices can act on vectors\n Eigen::Vector3d x;\n x << 0.0, 0.0, 1.0;\n std::cout << \"Rotation matrices can act on vectors\" << std::endl;\n std::cout << \"x\\n\" << x << std::endl;\n std::cout << \"R2*x\\n\" << R2 * x << std::endl;\n std::cout << \"R1*(R2*x)\\n\" << R1 * (R2 * x) << std::endl;\n std::cout << \"(R1*R2)*x\\n\" << (R1 * R2) * x << std::endl;\n std::cout << std::endl;\n\n // SO(3) are internally represented as unit quaternions.\n std::cout << \"R1 in matrix form:\\n\" << R1.matrix() << std::endl;\n std::cout << \"R1 in unit quaternion form:\\n\"\n << R1.unit_quaternion().coeffs() << std::endl;\n // Note that the order of coefficients of Eigen's quaternion class is\n // (imag0, imag1, imag2, real)\n std::cout << std::endl;\n}" }, { "path": "Thirdparty/Sophus/generate_stubs.py", "content": "import subprocess\n\nsubprocess.run(\n \"pybind11-stubgen sophus_pybind -o sophus_pybind-stubs/\",\n shell=True,\n check=True,\n)\n\nsubprocess.run(\"touch sophus_pybind-stubs/py.typed\", shell=True, check=True)\n" }, { "path": "Thirdparty/Sophus/make_docs.sh", "content": "doxygen doxyfile\ndoxyrest_b/build/doxyrest/bin/Release/doxyrest -c doxyrest-config.lua\nsphinx-build -b html rst-dir html-dir" }, { "path": "Thirdparty/Sophus/package.xml", "content": "\n\n sophus\n 1.22.10\n \n C++ implementation of Lie Groups using Eigen.\n \n https://github.com/strasdat/sophus\n https://github.com/strasdat/sophus/issues\n Daniel Stonier\n Hauke Strasdat\n MIT\n\n cmake\n\n eigen\n eigen\n\n \n cmake\n \n\n" }, { "path": "Thirdparty/Sophus/rst-dir/conf.py", "content": "# Configuration file for the Sphinx documentation builder.\n#\n# This file only contains a selection of the most common options. For a full\n# list see the documentation:\n# http://www.sphinx-doc.org/en/master/config\n\n# -- Path setup --------------------------------------------------------------\n\n# If extensions (or modules to document with autodoc) are in another directory,\n# add these directories to sys.path here. If the directory is relative to the\n# documentation root, use os.path.abspath to make it absolute, like shown here.\n#\nimport os\nimport sys\nsys.path.insert(0, os.path.abspath('../sympy'))\n\n\nsys.path.insert(1, os.path.abspath('../doxyrest_b/doxyrest/sphinx'))\nextensions = ['doxyrest', 'cpplexer', 'sphinx.ext.autodoc']\n\n# -- Project information -----------------------------------------------------\n\nproject = 'Sophus'\ncopyright = '2019, Hauke Strasdat'\nauthor = 'Hauke Strasdat'\n\n\n# Tell sphinx what the primary language being documented is.\nprimary_domain = 'cpp'\n\n# Tell sphinx what the pygments highlight language should be.\nhighlight_language = 'cpp'\n\n\n# Add any paths that contain templates here, relative to this directory.\ntemplates_path = ['_templates']\n\n# List of patterns, relative to source directory, that match files and\n# directories to ignore when looking for source files.\n# This pattern also affects html_static_path and html_extra_path.\nexclude_patterns = []\n\n\n# -- Options for HTML output -------------------------------------------------\n\n# The theme to use for HTML and HTML Help pages. See the documentation for\n# a list of builtin themes.\n#\nhtml_theme = \"sphinx_rtd_theme\"\n\n# Add any paths that contain custom static files (such as style sheets) here,\n# relative to this directory. They are copied after the builtin static files,\n# so a file named \"default.css\" will overwrite the builtin \"default.css\".\nhtml_static_path = ['_static']\n" }, { "path": "Thirdparty/Sophus/rst-dir/page_index.rst", "content": "Sophus - Lie groups for 2d/3d Geometry\n=======================================\n\n.. toctree::\n :maxdepth: 2\n :caption: Contents:\n\n GitHub Page \n pysophus" }, { "path": "Thirdparty/Sophus/rst-dir/pysophus.rst", "content": "Python API\n==========\n\n.. automodule:: sophus.matrix\n :members:\n\n.. automodule:: sophus.complex\n :members:\n\n.. automodule:: sophus.quaternion\n :members:\n\n.. automodule:: sophus.so2\n :members:\n\n.. automodule:: sophus.so3\n :members:\n\n.. automodule:: sophus.se2\n :members:\n\n.. automodule:: sophus.se3\n :members:" }, { "path": "Thirdparty/Sophus/run_format.sh", "content": "find . -type d \\( -path ./sympy -o -path ./doxyrest_b -o -path \"./*/CMakeFiles/*\" \\) -prune -o \\( -iname \"*.hpp\" -o -iname \"*.cpp\" \\) -print | xargs clang-format -i\n" }, { "path": "Thirdparty/Sophus/scripts/install_docs_deps.sh", "content": "#!/bin/bash\n\nset -x # echo on\nset -e # exit on error\n\nsudo apt-get -qq update\nsudo apt-get install doxygen liblua5.3-dev ragel\npip3 install 'sphinx==2.0.1'\npip3 install sphinx_rtd_theme\npip3 install sympy\n\ngit clone https://github.com/vovkos/doxyrest_b\ncd doxyrest_b\ngit reset --hard ad45c064d1199e71b8cae5aa66d4251c4228b958\ngit submodule update --init\nmkdir build\ncd build\ncmake ..\ncmake --build .\n\ncd ../..\n" }, { "path": "Thirdparty/Sophus/scripts/install_linux_deps.sh", "content": "#!/bin/bash\n\nset -x # echo on\nset -e # exit on error\n\ncmake --version\n\nsudo apt-get -qq update\nsudo apt-get install gfortran libc++-dev libgoogle-glog-dev libatlas-base-dev libsuitesparse-dev libceres-dev ccache\nwget https://gitlab.com/libeigen/eigen/-/archive/3.3.4/eigen-3.3.4.tar.bz2\ntar xvf eigen-3.3.4.tar.bz2\nmkdir build-eigen\ncd build-eigen\ncmake ../eigen-3.3.4 -DEIGEN_DEFAULT_TO_ROW_MAJOR=$ROW_MAJOR_DEFAULT\nsudo make install\ncd ..\n\ngit clone https://ceres-solver.googlesource.com/ceres-solver ceres-solver\ncd ceres-solver\ngit reset --hard b0aef211db734379319c19c030e734d6e23436b0\nmkdir build\ncd build\nccache -s\ncmake -DCMAKE_CXX_COMPILER_LAUNCHER=ccache ..\nmake -j8\nsudo make install\ncd ../..\n" }, { "path": "Thirdparty/Sophus/scripts/install_linux_fmt_deps.sh", "content": "#!/bin/bash\n\nset -x # echo on\nset -e # exit on error\n\ngit clone https://github.com/fmtlib/fmt.git\ncd fmt\ngit checkout 5.3.0\nmkdir build\ncd build\ncmake -DCMAKE_CXX_COMPILER_LAUNCHER=ccache ..\nmake -j8\nsudo make install\ncd ../..\n" }, { "path": "Thirdparty/Sophus/scripts/install_osx_deps.sh", "content": "#!/bin/bash\n\nset -x # echo on\nset -e # exit on error\n\nbrew update\nbrew install fmt\nbrew install ccache\n\n# Build a specific version of ceres-solver instead of one shipped over brew\ncurl https://raw.githubusercontent.com/Homebrew/homebrew-core/b0792ccba6e71cd028263ca7621db894afc602d2/Formula/ceres-solver.rb -o ceres-solver.rb\npatch <=3.6\",\n install_requires=[\"numpy\"],\n packages=find_packages(),\n license=\"Apache-2.0\",\n )\n\n\nif __name__ == \"__main__\":\n main()\n" }, { "path": "Thirdparty/Sophus/sophus/average.hpp", "content": "/// @file\n/// Calculation of biinvariant means.\n\n#pragma once\n\n#include \n\n#include \"cartesian.hpp\"\n#include \"common.hpp\"\n#include \"rxso2.hpp\"\n#include \"rxso3.hpp\"\n#include \"se2.hpp\"\n#include \"se3.hpp\"\n#include \"sim2.hpp\"\n#include \"sim3.hpp\"\n#include \"so2.hpp\"\n#include \"so3.hpp\"\n\nnamespace Sophus {\n\n/// Calculates mean iteratively.\n///\n/// Returns ``nullopt`` if it does not converge.\n///\ntemplate \noptional iterativeMean(\n SequenceContainer const& foo_Ts_bar, int max_num_iterations) {\n size_t N = foo_Ts_bar.size();\n SOPHUS_ENSURE(N >= 1, \"N must be >= 1.\");\n\n using Group = typename SequenceContainer::value_type;\n using Scalar = typename Group::Scalar;\n using Tangent = typename Group::Tangent;\n\n // This implements the algorithm in the beginning of Sec. 4.2 in\n // ftp://ftp-sop.inria.fr/epidaure/Publications/Arsigny/arsigny_rr_biinvariant_average.pdf.\n Group foo_T_average = foo_Ts_bar.front();\n Scalar w = Scalar(1. / N);\n for (int i = 0; i < max_num_iterations; ++i) {\n Tangent average;\n setToZero(average);\n for (Group const& foo_T_bar : foo_Ts_bar) {\n average += w * (foo_T_average.inverse() * foo_T_bar).log();\n }\n Group foo_T_newaverage = foo_T_average * Group::exp(average);\n if (squaredNorm(\n (foo_T_newaverage.inverse() * foo_T_average).log()) <\n Constants::epsilon()) {\n return foo_T_newaverage;\n }\n\n foo_T_average = foo_T_newaverage;\n }\n // LCOV_EXCL_START\n return nullopt;\n // LCOV_EXCL_STOP\n}\n\n#ifdef DOXYGEN_SHOULD_SKIP_THIS\n/// Mean implementation for any Lie group.\ntemplate \noptional average(\n SequenceContainer const& foo_Ts_bar);\n#else\n\n// Mean implementation for Cartesian.\ntemplate \nenable_if_t >::value,\n optional >\naverage(SequenceContainer const& foo_Ts_bar) {\n size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar));\n SOPHUS_ENSURE(N >= 1, \"N must be >= 1.\");\n\n Sophus::Vector average;\n average.setZero();\n for (Cartesian const& foo_T_bar : foo_Ts_bar) {\n average += foo_T_bar.params();\n }\n return Cartesian(average / Scalar(N));\n}\n\n// Mean implementation for SO(2).\ntemplate \nenable_if_t<\n std::is_same >::value,\n optional >\naverage(SequenceContainer const& foo_Ts_bar) {\n // This implements rotational part of Proposition 12 from Sec. 6.2 of\n // ftp://ftp-sop.inria.fr/epidaure/Publications/Arsigny/arsigny_rr_biinvariant_average.pdf.\n size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar));\n SOPHUS_ENSURE(N >= 1, \"N must be >= 1.\");\n SO2 foo_T_average = foo_Ts_bar.front();\n Scalar w = Scalar(1. / N);\n\n Scalar average(0);\n for (SO2 const& foo_T_bar : foo_Ts_bar) {\n average += w * (foo_T_average.inverse() * foo_T_bar).log();\n }\n return foo_T_average * SO2::exp(average);\n}\n\n// Mean implementation for RxSO(2).\ntemplate \nenable_if_t<\n std::is_same >::value,\n optional >\naverage(SequenceContainer const& foo_Ts_bar) {\n size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar));\n SOPHUS_ENSURE(N >= 1, \"N must be >= 1.\");\n RxSO2 foo_T_average = foo_Ts_bar.front();\n Scalar w = Scalar(1. / N);\n\n Vector2 average(Scalar(0), Scalar(0));\n for (RxSO2 const& foo_T_bar : foo_Ts_bar) {\n average += w * (foo_T_average.inverse() * foo_T_bar).log();\n }\n return foo_T_average * RxSO2::exp(average);\n}\n\nnamespace details {\ntemplate \nvoid getQuaternion(T const&);\n\ntemplate \nEigen::Quaternion getUnitQuaternion(SO3 const& R) {\n return R.unit_quaternion();\n}\n\ntemplate \nEigen::Quaternion getUnitQuaternion(RxSO3 const& sR) {\n return sR.so3().unit_quaternion();\n}\n\ntemplate \nEigen::Quaternion averageUnitQuaternion(\n SequenceContainer const& foo_Ts_bar) {\n // This: http://stackoverflow.com/a/27410865/1221742\n size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar));\n SOPHUS_ENSURE(N >= 1, \"N must be >= 1.\");\n Eigen::Matrix Q(4, N);\n int i = 0;\n Scalar w = Scalar(1. / N);\n for (auto const& foo_T_bar : foo_Ts_bar) {\n Q.col(i) = w * details::getUnitQuaternion(foo_T_bar).coeffs();\n ++i;\n }\n\n Eigen::Matrix QQt = Q * Q.transpose();\n // TODO: Figure out why we can't use SelfAdjointEigenSolver here.\n Eigen::EigenSolver > es(QQt);\n\n std::complex max_eigenvalue = es.eigenvalues()[0];\n Eigen::Matrix, 4, 1> max_eigenvector =\n es.eigenvectors().col(0);\n\n for (int i = 1; i < 4; i++) {\n if (std::norm(es.eigenvalues()[i]) > std::norm(max_eigenvalue)) {\n max_eigenvalue = es.eigenvalues()[i];\n max_eigenvector = es.eigenvectors().col(i);\n }\n }\n Eigen::Quaternion quat;\n quat.coeffs() << //\n max_eigenvector[0].real(), //\n max_eigenvector[1].real(), //\n max_eigenvector[2].real(), //\n max_eigenvector[3].real();\n return quat;\n}\n} // namespace details\n\n// Mean implementation for SO(3).\n//\n// TODO: Detect degenerated cases and return nullopt.\ntemplate \nenable_if_t<\n std::is_same >::value,\n optional >\naverage(SequenceContainer const& foo_Ts_bar) {\n return SO3(details::averageUnitQuaternion(foo_Ts_bar));\n}\n\n// Mean implementation for R x SO(3).\ntemplate \nenable_if_t<\n std::is_same >::value,\n optional >\naverage(SequenceContainer const& foo_Ts_bar) {\n size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar));\n\n SOPHUS_ENSURE(N >= 1, \"N must be >= 1.\");\n Scalar scale_sum = Scalar(0);\n using std::exp;\n using std::log;\n for (RxSO3 const& foo_T_bar : foo_Ts_bar) {\n scale_sum += log(foo_T_bar.scale());\n }\n return RxSO3(exp(scale_sum / Scalar(N)),\n SO3(details::averageUnitQuaternion(foo_Ts_bar)));\n}\n\ntemplate \nenable_if_t<\n std::is_same >::value,\n optional >\naverage(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) {\n // TODO: Implement Proposition 12 from Sec. 6.2 of\n // ftp://ftp-sop.inria.fr/epidaure/Publications/Arsigny/arsigny_rr_biinvariant_average.pdf.\n return iterativeMean(foo_Ts_bar, max_num_iterations);\n}\n\ntemplate \nenable_if_t<\n std::is_same >::value,\n optional >\naverage(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) {\n return iterativeMean(foo_Ts_bar, max_num_iterations);\n}\n\ntemplate \nenable_if_t<\n std::is_same >::value,\n optional >\naverage(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) {\n return iterativeMean(foo_Ts_bar, max_num_iterations);\n}\n\ntemplate \nenable_if_t<\n std::is_same >::value,\n optional >\naverage(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) {\n return iterativeMean(foo_Ts_bar, max_num_iterations);\n}\n\n#endif // DOXYGEN_SHOULD_SKIP_THIS\n\n} // namespace Sophus\n" }, { "path": "Thirdparty/Sophus/sophus/cartesian.hpp", "content": "/// @file\n/// Cartesian - Euclidean vector space as Lie group\n\n#pragma once\n#include \n\nnamespace Sophus {\ntemplate \nclass Cartesian;\n\ntemplate \nusing Cartesian2 = Cartesian;\n\ntemplate \nusing Cartesian3 = Cartesian;\n\nusing Cartesian2d = Cartesian2;\nusing Cartesian3d = Cartesian3;\n\n} // namespace Sophus\n\nnamespace Eigen {\nnamespace internal {\n\ntemplate \nstruct traits> {\n using Scalar = Scalar_;\n using ParamsType = Sophus::Vector;\n};\n\ntemplate \nstruct traits, Options>>\n : traits> {\n using Scalar = Scalar_;\n using ParamsType = Map, Options>;\n};\n\ntemplate \nstruct traits const, Options>>\n : traits const> {\n using Scalar = Scalar_;\n using ParamsType = Map const, Options>;\n};\n} // namespace internal\n} // namespace Eigen\n\nnamespace Sophus {\n\n/// Cartesian base type - implements Cartesian class but is storage agnostic.\n///\n/// Euclidean vector space as Lie group.\n///\n/// Lie groups can be seen as a generalization over the Euclidean vector\n/// space R^M. Here a N-dimensional vector ``p`` is represented as a\n// (M+1) x (M+1) homogeneous matrix:\n///\n/// | I p |\n/// | o 1 |\n///\n/// On the other hand, Cartesian(M) can be seen as a special case of SE(M)\n/// with identity rotation, and hence represents pure translation.\n///\n/// The purpose of this class is two-fold:\n/// - for educational purpose, to highlight how Lie groups generalize over\n/// Euclidean vector spaces.\n/// - to be used in templated/generic algorithms (such as Sophus::Spline)\n/// which are implemented against the Lie group interface.\n///\n/// Obviously, Cartesian(M) can just be represented as a M-tuple.\n///\n/// Cartesian is not compact, but a commutative group. For vector additions it\n/// holds `a+b = b+a`.\n///\n/// See Cartesian class for more details below.\n///\ntemplate \nclass CartesianBase {\n public:\n using Scalar = typename Eigen::internal::traits::Scalar;\n using ParamsType = typename Eigen::internal::traits::ParamsType;\n /// Degrees of freedom of manifold, equals to number of Cartesian coordinates.\n static int constexpr DoF = M;\n /// Number of internal parameters used, also M.\n static int constexpr num_parameters = M;\n /// Group transformations are (M+1)x(M+1) matrices.\n static int constexpr N = M + 1;\n static int constexpr Dim = M;\n\n using Transformation = Sophus::Matrix;\n using Point = Sophus::Vector;\n using HomogeneousPoint = Sophus::Vector;\n using Line = ParametrizedLine;\n using Hyperplane = Eigen::Hyperplane;\n using Tangent = Sophus::Vector;\n using Adjoint = Matrix;\n\n /// For binary operations the return type is determined with the\n /// ScalarBinaryOpTraits feature of Eigen. This allows mixing concrete and Map\n /// types, as well as other compatible scalar types such as Ceres::Jet and\n /// double scalars with Cartesian operations.\n template \n using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<\n Scalar, typename OtherDerived::Scalar>::ReturnType;\n\n template \n using CartesianSum = Cartesian, M>;\n\n template \n using PointProduct = Sophus::Vector, M>;\n\n template \n using HomogeneousPointProduct =\n Sophus::Vector, N>;\n\n /// Adjoint transformation\n ///\n /// Always identity of commutative groups.\n SOPHUS_FUNC Adjoint Adj() const { return Adjoint::Identity(); }\n\n /// Returns copy of instance casted to NewScalarType.\n ///\n template \n SOPHUS_FUNC Cartesian cast() const {\n return Cartesian(params().template cast());\n }\n\n /// Returns derivative of this * exp(x) wrt x at x=0.\n ///\n SOPHUS_FUNC Matrix Dx_this_mul_exp_x_at_0()\n const {\n Sophus::Matrix m;\n m.setIdentity();\n return m;\n }\n\n /// Returns derivative of log(this^{-1} * x) by x at x=this.\n ///\n SOPHUS_FUNC Matrix Dx_log_this_inv_by_x_at_this()\n const {\n Matrix m;\n m.setIdentity();\n return m;\n }\n\n /// Returns group inverse.\n ///\n /// The additive inverse.\n ///\n SOPHUS_FUNC Cartesian inverse() const {\n return Cartesian(-params());\n }\n\n /// Logarithmic map\n ///\n /// For Euclidean vector space, just the identity. Or to be more precise\n /// it just extracts the significant M-vector from the NxN matrix.\n ///\n SOPHUS_FUNC Tangent log() const { return params(); }\n\n /// Returns 4x4 matrix representation of the instance.\n ///\n /// It has the following form:\n ///\n /// | I p |\n /// | o 1 |\n ///\n SOPHUS_FUNC Transformation matrix() const {\n Sophus::Matrix matrix;\n matrix.setIdentity();\n matrix.col(M).template head() = params();\n return matrix;\n }\n\n /// Group multiplication, are vector additions.\n ///\n template \n SOPHUS_FUNC CartesianBase& operator=(\n CartesianBase const& other) {\n params() = other.params();\n return *this;\n }\n\n /// Group multiplication, are vector additions.\n ///\n template \n SOPHUS_FUNC CartesianSum operator*(\n CartesianBase const& other) const {\n return CartesianSum(params() + other.params());\n }\n\n /// Group action on points, again just vector addition.\n ///\n template ::value>::type>\n SOPHUS_FUNC PointProduct operator*(\n Eigen::MatrixBase const& p) const {\n return PointProduct(params() + p);\n }\n\n /// Group action on homogeneous points. See above for more details.\n ///\n template ::value>::type>\n SOPHUS_FUNC HomogeneousPointProduct operator*(\n Eigen::MatrixBase const& p) const {\n const auto rp = *this * p.template head();\n HomogeneousPointProduct r;\n r << rp, p(M);\n return r;\n }\n\n /// Group action on lines.\n ///\n SOPHUS_FUNC Line operator*(Line const& l) const {\n return Line((*this) * l.origin(), l.direction());\n }\n\n /// Group action on planes.\n ///\n SOPHUS_FUNC Hyperplane operator*(Hyperplane const& p) const {\n return Hyperplane(p.normal(), p.offset() - params().dot(p.normal()));\n }\n\n /// In-place group multiplication. This method is only valid if the return\n /// type of the multiplication is compatible with this Cartesian's Scalar\n /// type.\n ///\n template >::value>::type>\n SOPHUS_FUNC CartesianBase& operator*=(\n CartesianBase const& other) {\n *static_cast(this) = *this * other;\n return *this;\n }\n\n /// Mutator of params vector.\n ///\n SOPHUS_FUNC ParamsType& params() {\n return static_cast(this)->params();\n }\n\n /// Accessor of params vector\n ///\n SOPHUS_FUNC ParamsType const& params() const {\n return static_cast(this)->params();\n }\n};\n\n/// Cartesian using default storage; derived from CartesianBase.\ntemplate \nclass Cartesian : public CartesianBase, M> {\n using Base = CartesianBase, M>;\n\n public:\n static int constexpr DoF = Base::DoF;\n static int constexpr num_parameters = Base::num_parameters;\n static int constexpr N = Base::N;\n static int constexpr Dim = Base::Dim;\n\n using Scalar = Scalar_;\n using Transformation = typename Base::Transformation;\n using Point = typename Base::Point;\n using HomogeneousPoint = typename Base::HomogeneousPoint;\n using Tangent = typename Base::Tangent;\n using ParamsMember = Sophus::Vector;\n\n using Base::operator=;\n\n /// Define copy-assignment operator explicitly. The definition of\n /// implicit copy assignment operator is deprecated in presence of a\n /// user-declared copy constructor (-Wdeprecated-copy in clang >= 13).\n SOPHUS_FUNC Cartesian& operator=(Cartesian const& other) = default;\n\n EIGEN_MAKE_ALIGNED_OPERATOR_NEW\n\n /// Default constructor initializes to zero vector.\n ///\n SOPHUS_FUNC Cartesian() { params_.setZero(); }\n\n /// Copy constructor\n ///\n SOPHUS_FUNC Cartesian(Cartesian const& other) = default;\n\n /// Copy-like constructor from OtherDerived.\n ///\n template \n SOPHUS_FUNC Cartesian(CartesianBase const& other)\n : params_(other.params()) {\n static_assert(std::is_same::value,\n \"must be same Scalar type\");\n }\n\n /// Accepts either M-vector or (M+1)x(M+1) matrices.\n ///\n template \n explicit SOPHUS_FUNC Cartesian(Eigen::MatrixBase const& m) {\n static_assert(\n std::is_same::Scalar, Scalar>::value, \"\");\n if (m.rows() == DoF && m.cols() == 1) {\n // trick so this compiles\n params_ = m.template block(0, 0);\n } else if (m.rows() == N && m.cols() == N) {\n params_ = m.template block(0, M);\n } else {\n SOPHUS_ENSURE(false, \"{} {}\", m.rows(), m.cols());\n }\n }\n\n /// This provides unsafe read/write access to internal data.\n ///\n SOPHUS_FUNC Scalar* data() { return params_.data(); }\n\n /// Const version of data() above.\n ///\n SOPHUS_FUNC Scalar const* data() const { return params_.data(); }\n\n /// Returns derivative of exp(x) wrt. x.\n ///\n SOPHUS_FUNC static Sophus::Matrix\n Dx_exp_x_at_0() {\n Sophus::Matrix m;\n m.setIdentity();\n return m;\n }\n\n /// Returns derivative of exp(x) wrt. x_i at x=0.\n ///\n SOPHUS_FUNC static Sophus::Matrix Dx_exp_x(\n Tangent const&) {\n return Dx_exp_x_at_0();\n }\n\n /// Returns derivative of exp(x) * p wrt. x_i at x=0.\n ///\n SOPHUS_FUNC static Sophus::Matrix Dx_exp_x_times_point_at_0(\n Point const&) {\n Sophus::Matrix J;\n J.setIdentity();\n return J;\n }\n\n /// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``.\n ///\n SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {\n return generator(i);\n }\n\n /// Mutator of params vector\n ///\n SOPHUS_FUNC ParamsMember& params() { return params_; }\n\n /// Accessor of params vector\n ///\n SOPHUS_FUNC ParamsMember const& params() const { return params_; }\n\n /// Returns the ith infinitesimal generators of Cartesian(M).\n ///\n /// The infinitesimal generators for e.g. the 3-dimensional case:\n ///\n /// ```\n /// | 0 0 0 1 |\n /// G_0 = | 0 0 0 0 |\n /// | 0 0 0 0 |\n /// | 0 0 0 0 |\n ///\n /// | 0 0 0 0 |\n /// G_1 = | 0 0 0 1 |\n /// | 0 0 0 0 |\n /// | 0 0 0 0 |\n ///\n /// | 0 0 0 0 |\n /// G_2 = | 0 0 0 0 |\n /// | 0 0 0 1 |\n /// | 0 0 0 0 |\n /// ```\n ///\n /// Precondition: ``i`` must be in [0, M-1].\n ///\n SOPHUS_FUNC static Transformation generator(int i) {\n SOPHUS_ENSURE(i >= 0 && i <= M, \"i should be in range [0,M-1].\");\n Tangent e;\n e.setZero();\n e[i] = Scalar(1);\n return hat(e);\n }\n\n /// Group exponential\n ///\n /// For Euclidean vector space, just the identity. Or to be more precise\n /// it just constructs the (M+1xM+1) homogeneous matrix representation\n // from the M-vector.\n ///\n SOPHUS_FUNC static Cartesian exp(Tangent const& a) {\n return Cartesian(a);\n }\n\n /// hat-operator\n ///\n /// Formally, the hat()-operator of Cartesian(M) is defined as\n ///\n /// ``hat(.): R^M -> R^{M+1xM+1}, hat(a) = sum_i a_i * G_i``\n /// (for i=0,...,M-1)\n ///\n /// with ``G_i`` being the ith infinitesimal generator of Cartesian(M).\n ///\n /// The corresponding inverse is the vee()-operator, see below.\n ///\n SOPHUS_FUNC static Transformation hat(Tangent const& a) {\n Transformation Omega;\n Omega.setZero();\n Omega.col(M).template head() = a.template head();\n return Omega;\n }\n\n /// Lie bracket\n ///\n /// Always 0 for commutative groups.\n SOPHUS_FUNC static Tangent lieBracket(Tangent const&, Tangent const&) {\n return Tangent::Zero();\n }\n\n /// Draws uniform samples in the range [-1, 1] per coordinates.\n ///\n template \n static Cartesian sampleUniform(UniformRandomBitGenerator& generator) {\n std::uniform_real_distribution uniform(Scalar(-1), Scalar(1));\n Vector v;\n for (int i = 0; i < M; ++i) {\n v[i] = uniform(generator);\n }\n return Cartesian(v);\n }\n\n /// vee-operator\n ///\n /// This is the inverse of the hat()-operator, see above.\n ///\n SOPHUS_FUNC static Tangent vee(Transformation const& m) {\n return m.col(M).template head();\n }\n\n protected:\n ParamsMember params_;\n};\n\n} // namespace Sophus\n\nnamespace Eigen {\n\n/// Specialization of Eigen::Map for ``Cartesian``; derived from\n/// CartesianBase.\n///\n/// Allows us to wrap Cartesian objects around POD array.\ntemplate \nclass Map, Options>\n : public Sophus::CartesianBase, Options>,\n M> {\n public:\n using Base =\n Sophus::CartesianBase, Options>, M>;\n using Scalar = Scalar_;\n using Transformation = typename Base::Transformation;\n using Point = typename Base::Point;\n using HomogeneousPoint = typename Base::HomogeneousPoint;\n using Tangent = typename Base::Tangent;\n\n using Base::operator=;\n using Base::operator*=;\n using Base::operator*;\n\n SOPHUS_FUNC explicit Map(Scalar* coeffs) : params_(coeffs) {}\n\n /// Mutator of params vector\n ///\n SOPHUS_FUNC Map>& params() {\n return params_;\n }\n\n /// Accessor of params vector\n ///\n SOPHUS_FUNC Map> const& params() const {\n return params_;\n }\n\n protected:\n Map, Options> params_;\n};\n\n/// Specialization of Eigen::Map for ``Cartesian const``; derived from\n/// CartesianBase.\n///\n/// Allows us to wrap Cartesian objects around POD array.\ntemplate \nclass Map const, Options>\n : public Sophus::CartesianBase<\n Map const, Options>, M> {\n public:\n using Base =\n Sophus::CartesianBase const, Options>,\n M>;\n using Scalar = Scalar_;\n using Transformation = typename Base::Transformation;\n using Point = typename Base::Point;\n using HomogeneousPoint = typename Base::HomogeneousPoint;\n using Tangent = typename Base::Tangent;\n\n using Base::operator*;\n\n SOPHUS_FUNC Map(Scalar const* coeffs) : params_(coeffs) {}\n\n /// Accessor of params vector\n ///\n SOPHUS_FUNC Map const, Options> const& params()\n const {\n return params_;\n }\n\n protected:\n Map const, Options> const params_;\n};\n} // namespace Eigen\n" }, { "path": "Thirdparty/Sophus/sophus/ceres_local_parameterization.hpp", "content": "#pragma once\n\n#include \n\n#include \n\nnamespace Sophus {\n\n/// Templated local parameterization for LieGroup [with implemented\n/// LieGroup::Dx_this_mul_exp_x_at_0() ]\ntemplate