diff --git a/README.md b/README.md index de45f37d..f44f2b00 100644 --- a/README.md +++ b/README.md @@ -48,8 +48,6 @@ Note: Rows within each cell are for ``amd64`` and ``arm64`` architectures. | mola_launcher | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_launcher__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_launcher__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_launcher__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_launcher__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_launcher__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_launcher__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_launcher__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_launcher__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_launcher__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_launcher__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_launcher__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_launcher__ubuntu_noble_arm64__binary/) | | mola_metric_maps | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_metric_maps__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_metric_maps__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_metric_maps__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_metric_maps__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_metric_maps__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_metric_maps__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_metric_maps__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_metric_maps__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_metric_maps__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_metric_maps__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_metric_maps__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_metric_maps__ubuntu_noble_arm64__binary/) | | mola_msgs | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_msgs__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_msgs__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_msgs__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_msgs__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_msgs__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_msgs__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_msgs__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_msgs__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_msgs__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_msgs__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_msgs__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_msgs__ubuntu_noble_arm64__binary/) | -| mola_navstate_fg | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_navstate_fg__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_navstate_fg__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_navstate_fg__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_navstate_fg__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_navstate_fg__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_navstate_fg__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_navstate_fg__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_navstate_fg__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_navstate_fg__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_navstate_fg__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_navstate_fg__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_navstate_fg__ubuntu_noble_arm64__binary/) | -| mola_navstate_fuse | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_navstate_fuse__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_navstate_fuse__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_navstate_fuse__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_navstate_fuse__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_navstate_fuse__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_navstate_fuse__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_navstate_fuse__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_navstate_fuse__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_navstate_fuse__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_navstate_fuse__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_navstate_fuse__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_navstate_fuse__ubuntu_noble_arm64__binary/) | | mola_pose_list | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_pose_list__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_pose_list__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_pose_list__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_pose_list__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_pose_list__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_pose_list__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_pose_list__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_pose_list__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_pose_list__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_pose_list__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_pose_list__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_pose_list__ubuntu_noble_arm64__binary/) | | mola_relocalization | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_relocalization__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_relocalization__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_relocalization__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_relocalization__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_relocalization__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_relocalization__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_relocalization__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_relocalization__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_relocalization__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_relocalization__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_relocalization__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_relocalization__ubuntu_noble_arm64__binary/) | | mola_traj_tools | [![Build Status](https://build.ros2.org/job/Hbin_uJ64__mola_traj_tools__ubuntu_jammy_amd64__binary/badge/icon)](https://build.ros2.org/job/Hbin_uJ64__mola_traj_tools__ubuntu_jammy_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_traj_tools__ubuntu_jammy_arm64__binary/badge/icon)](https://build.ros2.org/job/Hbin_ujv8_uJv8__mola_traj_tools__ubuntu_jammy_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Jbin_uN64__mola_traj_tools__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Jbin_uN64__mola_traj_tools__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_traj_tools__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Jbin_unv8_uNv8__mola_traj_tools__ubuntu_noble_arm64__binary/) | [![Build Status](https://build.ros2.org/job/Rbin_uN64__mola_traj_tools__ubuntu_noble_amd64__binary/badge/icon)](https://build.ros2.org/job/Rbin_uN64__mola_traj_tools__ubuntu_noble_amd64__binary/)
[![Build Status](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_traj_tools__ubuntu_noble_arm64__binary/badge/icon)](https://build.ros2.org/job/Rbin_unv8_uNv8__mola_traj_tools__ubuntu_noble_arm64__binary/) | diff --git a/mola/package.xml b/mola/package.xml index 12d8a74d..b63bf469 100644 --- a/mola/package.xml +++ b/mola/package.xml @@ -24,8 +24,6 @@ mola_kernel mola_launcher mola_metric_maps - mola_navstate_fg - mola_navstate_fuse mola_pose_list mola_relocalization mola_traj_tools diff --git a/mola_navstate_fg/.clang-format b/mola_navstate_fg/.clang-format deleted file mode 100644 index d1df9d28..00000000 --- a/mola_navstate_fg/.clang-format +++ /dev/null @@ -1,83 +0,0 @@ -Language: Cpp -BasedOnStyle: Google -# --- -#AccessModifierOffset: -4 -AlignAfterOpenBracket: AlwaysBreak # Values: Align, DontAlign, AlwaysBreak -AlignConsecutiveAssignments: true -AlignConsecutiveDeclarations: true -#AlignEscapedNewlinesLeft: true -#AlignOperands: false -AlignTrailingComments: false # Should be off, causes many dummy problems!! -#AllowAllParametersOfDeclarationOnNextLine: true -AllowShortBlocksOnASingleLine: true -#AllowShortCaseLabelsOnASingleLine: false -#AllowShortFunctionsOnASingleLine: Empty -#AllowShortIfStatementsOnASingleLine: false -#AllowShortLoopsOnASingleLine: false -#AlwaysBreakAfterDefinitionReturnType: None -#AlwaysBreakAfterReturnType: None -#AlwaysBreakBeforeMultilineStrings: true -#AlwaysBreakTemplateDeclarations: true -#BinPackArguments: false -#BinPackParameters: false -#BraceWrapping: - #AfterClass: false - #AfterControlStatement: false - #AfterEnum: false - #AfterFunction: false - #AfterNamespace: false - #AfterObjCDeclaration: false - #AfterStruct: false - #AfterUnion: false - #BeforeCatch: false - #BeforeElse: true - #IndentBraces: false -#BreakBeforeBinaryOperators: None -BreakBeforeBraces: Allman -#BreakBeforeTernaryOperators: true -#BreakConstructorInitializersBeforeComma: false -ColumnLimit: 80 -#CommentPragmas: '' -#ConstructorInitializerAllOnOneLineOrOnePerLine: true -#ConstructorInitializerIndentWidth: 4 -#ContinuationIndentWidth: 4 -#Cpp11BracedListStyle: true -#DerivePointerAlignment: false -#DisableFormat: false -#ExperimentalAutoDetectBinPacking: false -##FixNamespaceComments: true # Not applicable in 3.8 -#ForEachMacros: [ foreach, Q_FOREACH, BOOST_FOREACH ] -#IncludeCategories: - #- Regex: '.*' - #Priority: 1 -IndentCaseLabels: true -IndentWidth: 4 -IndentWrappedFunctionNames: true -#KeepEmptyLinesAtTheStartOfBlocks: true -#MacroBlockBegin: '' -#MacroBlockEnd: '' -MaxEmptyLinesToKeep: 1 -NamespaceIndentation: None -#PenaltyBreakBeforeFirstCallParameter: 19 -#PenaltyBreakComment: 300 -#PenaltyBreakFirstLessLess: 120 -#PenaltyBreakString: 1000 -#PenaltyExcessCharacter: 1000000 -#PenaltyReturnTypeOnItsOwnLine: 200 -DerivePointerAlignment: false -#PointerAlignment: Left -ReflowComments: true # Should be true, otherwise clang-format doesn't touch comments -SortIncludes: true -#SpaceAfterCStyleCast: false -SpaceBeforeAssignmentOperators: true -#SpaceBeforeParens: ControlStatements -#SpaceInEmptyParentheses: false -#SpacesBeforeTrailingComments: 2 -#SpacesInAngles: false -#SpacesInContainerLiterals: true -#SpacesInCStyleCastParentheses: false -#SpacesInParentheses: false -#SpacesInSquareBrackets: false -Standard: Cpp11 -TabWidth: 4 -UseTab: Never # Available options are Never, Always, ForIndentation diff --git a/mola_navstate_fg/CHANGELOG.rst b/mola_navstate_fg/CHANGELOG.rst deleted file mode 100644 index aae3084d..00000000 --- a/mola_navstate_fg/CHANGELOG.rst +++ /dev/null @@ -1,82 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package mola_navstate_fg -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -1.4.1 (2024-12-20) ------------------- - -1.4.0 (2024-12-18) ------------------- - -1.3.0 (2024-12-11) ------------------- -* Start integrating GNSS observation. Added a new CLI program mola-navstate-cli for testing state fusion -* Contributors: Jose Luis Blanco-Claraco - -1.2.1 (2024-09-29) ------------------- - -1.2.0 (2024-09-16) ------------------- - -1.1.3 (2024-08-28) ------------------- -* Depend on new mrpt_lib packages (deprecate mrpt2) -* Contributors: Jose Luis Blanco-Claraco - -1.1.2 (2024-08-26) ------------------- - -1.1.1 (2024-08-23) ------------------- - -1.1.0 (2024-08-18) ------------------- -* Update test-navstate-basic.cpp: less noisy test data for more predictable results -* Merge pull request `#62 `_ from MOLAorg/docs-fixes - Docs fixes -* Fix ament_xmllint warnings in package.xml -* Contributors: Jose Luis Blanco-Claraco - -1.0.8 (2024-07-29) ------------------- -* ament_lint_cmake: clean warnings -* Contributors: Jose Luis Blanco-Claraco - -1.0.7 (2024-07-24) ------------------- -* Fix GNSS typo -* Contributors: Jose Luis Blanco-Claraco - -1.0.6 (2024-06-21) ------------------- -* Create new NavStateFilter interface and separate the simple fuser and the factor-graph approach in two packages -* Contributors: Jose Luis Blanco-Claraco - -1.0.5 (2024-05-28) ------------------- - -1.0.4 (2024-05-14) ------------------- -* bump cmake_minimum_required to 3.5 -* Contributors: Jose Luis Blanco-Claraco - -1.0.3 (2024-04-22) ------------------- -* Fix package.xml website URL -* Contributors: Jose Luis Blanco-Claraco - -1.0.2 (2024-04-04) ------------------- - -1.0.1 (2024-03-28) ------------------- - -1.0.0 (2024-03-19) ------------------- -* use odometry -* add new package mola_navstate_fuse -* Contributors: Jose Luis Blanco-Claraco - -0.2.2 (2023-09-08) ------------------- diff --git a/mola_navstate_fg/CMakeLists.txt b/mola_navstate_fg/CMakeLists.txt deleted file mode 100644 index 80878d28..00000000 --- a/mola_navstate_fg/CMakeLists.txt +++ /dev/null @@ -1,60 +0,0 @@ -# ------------------------------------------------------------------------------ -# A Modular Optimization framework for Localization and mApping -# (MOLA) -# -# Copyright (C) 2018-2024, Jose Luis Blanco-Claraco, contributors (AUTHORS.md) -# All rights reserved. -# Released under GNU GPL v3. See LICENSE file -# ------------------------------------------------------------------------------ - -# Minimum CMake vesion: limited by CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS -cmake_minimum_required(VERSION 3.5) - -# Tell CMake we'll use C++ for use in its tests/flags -project(mola_navstate_fg LANGUAGES CXX) - -# MOLA CMake scripts: "mola_xxx()" -find_package(mola_common REQUIRED) - -# find CMake dependencies: -find_package(mrpt-obs) -find_package(GTSAM REQUIRED) - -# Find MOLA packages: -find_package(mola_kernel REQUIRED) -find_package(mola_imu_preintegration REQUIRED) - -# ----------------------- -# define lib: -file(GLOB_RECURSE LIB_SRCS src/*.cpp src/*.h) -file(GLOB_RECURSE LIB_PUBLIC_HDRS include/*.h) - -mola_add_library( - TARGET ${PROJECT_NAME} - SOURCES ${LIB_SRCS} ${LIB_PUBLIC_HDRS} - PUBLIC_LINK_LIBRARIES - mrpt::obs - mola::mola_kernel - mola::mola_imu_preintegration - PRIVATE_LINK_LIBRARIES - gtsam - CMAKE_DEPENDENCIES - mola_common - mola_imu_preintegration - mola_kernel -) -target_include_directories(${PROJECT_NAME} PRIVATE ".") - -# ----------------------- -# define cli apps: -mola_add_executable( - TARGET mola-navstate-cli - SOURCES apps/mola-navstate-cli.cpp - LINK_LIBRARIES - ${PROJECT_NAME} -) - -# ----------------------- -# define tests: -enable_testing() -add_subdirectory(tests) diff --git a/mola_navstate_fg/LICENSE b/mola_navstate_fg/LICENSE deleted file mode 100644 index f288702d..00000000 --- a/mola_navstate_fg/LICENSE +++ /dev/null @@ -1,674 +0,0 @@ - GNU GENERAL PUBLIC LICENSE - Version 3, 29 June 2007 - - Copyright (C) 2007 Free Software Foundation, Inc. - Everyone is permitted to copy and distribute verbatim copies - of this license document, but changing it is not allowed. - - Preamble - - The GNU General Public License is a free, copyleft license for -software and other kinds of works. - - The licenses for most software and other practical works are designed -to take away your freedom to share and change the works. By contrast, -the GNU General Public License is intended to guarantee your freedom to -share and change all versions of a program--to make sure it remains free -software for all its users. We, the Free Software Foundation, use the -GNU General Public License for most of our software; it applies also to -any other work released this way by its authors. You can apply it to -your programs, too. - - When we speak of free software, we are referring to freedom, not -price. Our General Public Licenses are designed to make sure that you -have the freedom to distribute copies of free software (and charge for -them if you wish), that you receive source code or can get it if you -want it, that you can change the software or use pieces of it in new -free programs, and that you know you can do these things. - - To protect your rights, we need to prevent others from denying you -these rights or asking you to surrender the rights. Therefore, you have -certain responsibilities if you distribute copies of the software, or if -you modify it: responsibilities to respect the freedom of others. - - For example, if you distribute copies of such a program, whether -gratis or for a fee, you must pass on to the recipients the same -freedoms that you received. You must make sure that they, too, receive -or can get the source code. And you must show them these terms so they -know their rights. - - Developers that use the GNU GPL protect your rights with two steps: -(1) assert copyright on the software, and (2) offer you this License -giving you legal permission to copy, distribute and/or modify it. - - For the developers' and authors' protection, the GPL clearly explains -that there is no warranty for this free software. For both users' and -authors' sake, the GPL requires that modified versions be marked as -changed, so that their problems will not be attributed erroneously to -authors of previous versions. - - Some devices are designed to deny users access to install or run -modified versions of the software inside them, although the manufacturer -can do so. This is fundamentally incompatible with the aim of -protecting users' freedom to change the software. The systematic -pattern of such abuse occurs in the area of products for individuals to -use, which is precisely where it is most unacceptable. Therefore, we -have designed this version of the GPL to prohibit the practice for those -products. If such problems arise substantially in other domains, we -stand ready to extend this provision to those domains in future versions -of the GPL, as needed to protect the freedom of users. - - Finally, every program is threatened constantly by software patents. -States should not allow patents to restrict development and use of -software on general-purpose computers, but in those that do, we wish to -avoid the special danger that patents applied to a free program could -make it effectively proprietary. To prevent this, the GPL assures that -patents cannot be used to render the program non-free. - - The precise terms and conditions for copying, distribution and -modification follow. - - TERMS AND CONDITIONS - - 0. Definitions. - - "This License" refers to version 3 of the GNU General Public License. - - "Copyright" also means copyright-like laws that apply to other kinds of -works, such as semiconductor masks. - - "The Program" refers to any copyrightable work licensed under this -License. Each licensee is addressed as "you". "Licensees" and -"recipients" may be individuals or organizations. - - To "modify" a work means to copy from or adapt all or part of the work -in a fashion requiring copyright permission, other than the making of an -exact copy. The resulting work is called a "modified version" of the -earlier work or a work "based on" the earlier work. - - A "covered work" means either the unmodified Program or a work based -on the Program. - - To "propagate" a work means to do anything with it that, without -permission, would make you directly or secondarily liable for -infringement under applicable copyright law, except executing it on a -computer or modifying a private copy. Propagation includes copying, -distribution (with or without modification), making available to the -public, and in some countries other activities as well. - - To "convey" a work means any kind of propagation that enables other -parties to make or receive copies. Mere interaction with a user through -a computer network, with no transfer of a copy, is not conveying. - - An interactive user interface displays "Appropriate Legal Notices" -to the extent that it includes a convenient and prominently visible -feature that (1) displays an appropriate copyright notice, and (2) -tells the user that there is no warranty for the work (except to the -extent that warranties are provided), that licensees may convey the -work under this License, and how to view a copy of this License. If -the interface presents a list of user commands or options, such as a -menu, a prominent item in the list meets this criterion. - - 1. Source Code. - - The "source code" for a work means the preferred form of the work -for making modifications to it. "Object code" means any non-source -form of a work. - - A "Standard Interface" means an interface that either is an official -standard defined by a recognized standards body, or, in the case of -interfaces specified for a particular programming language, one that -is widely used among developers working in that language. - - The "System Libraries" of an executable work include anything, other -than the work as a whole, that (a) is included in the normal form of -packaging a Major Component, but which is not part of that Major -Component, and (b) serves only to enable use of the work with that -Major Component, or to implement a Standard Interface for which an -implementation is available to the public in source code form. A -"Major Component", in this context, means a major essential component -(kernel, window system, and so on) of the specific operating system -(if any) on which the executable work runs, or a compiler used to -produce the work, or an object code interpreter used to run it. - - The "Corresponding Source" for a work in object code form means all -the source code needed to generate, install, and (for an executable -work) run the object code and to modify the work, including scripts to -control those activities. However, it does not include the work's -System Libraries, or general-purpose tools or generally available free -programs which are used unmodified in performing those activities but -which are not part of the work. For example, Corresponding Source -includes interface definition files associated with source files for -the work, and the source code for shared libraries and dynamically -linked subprograms that the work is specifically designed to require, -such as by intimate data communication or control flow between those -subprograms and other parts of the work. - - The Corresponding Source need not include anything that users -can regenerate automatically from other parts of the Corresponding -Source. - - The Corresponding Source for a work in source code form is that -same work. - - 2. Basic Permissions. - - All rights granted under this License are granted for the term of -copyright on the Program, and are irrevocable provided the stated -conditions are met. This License explicitly affirms your unlimited -permission to run the unmodified Program. The output from running a -covered work is covered by this License only if the output, given its -content, constitutes a covered work. This License acknowledges your -rights of fair use or other equivalent, as provided by copyright law. - - You may make, run and propagate covered works that you do not -convey, without conditions so long as your license otherwise remains -in force. You may convey covered works to others for the sole purpose -of having them make modifications exclusively for you, or provide you -with facilities for running those works, provided that you comply with -the terms of this License in conveying all material for which you do -not control copyright. Those thus making or running the covered works -for you must do so exclusively on your behalf, under your direction -and control, on terms that prohibit them from making any copies of -your copyrighted material outside their relationship with you. - - Conveying under any other circumstances is permitted solely under -the conditions stated below. Sublicensing is not allowed; section 10 -makes it unnecessary. - - 3. Protecting Users' Legal Rights From Anti-Circumvention Law. - - No covered work shall be deemed part of an effective technological -measure under any applicable law fulfilling obligations under article -11 of the WIPO copyright treaty adopted on 20 December 1996, or -similar laws prohibiting or restricting circumvention of such -measures. - - When you convey a covered work, you waive any legal power to forbid -circumvention of technological measures to the extent such circumvention -is effected by exercising rights under this License with respect to -the covered work, and you disclaim any intention to limit operation or -modification of the work as a means of enforcing, against the work's -users, your or third parties' legal rights to forbid circumvention of -technological measures. - - 4. Conveying Verbatim Copies. - - You may convey verbatim copies of the Program's source code as you -receive it, in any medium, provided that you conspicuously and -appropriately publish on each copy an appropriate copyright notice; -keep intact all notices stating that this License and any -non-permissive terms added in accord with section 7 apply to the code; -keep intact all notices of the absence of any warranty; and give all -recipients a copy of this License along with the Program. - - You may charge any price or no price for each copy that you convey, -and you may offer support or warranty protection for a fee. - - 5. Conveying Modified Source Versions. - - You may convey a work based on the Program, or the modifications to -produce it from the Program, in the form of source code under the -terms of section 4, provided that you also meet all of these conditions: - - a) The work must carry prominent notices stating that you modified - it, and giving a relevant date. - - b) The work must carry prominent notices stating that it is - released under this License and any conditions added under section - 7. This requirement modifies the requirement in section 4 to - "keep intact all notices". - - c) You must license the entire work, as a whole, under this - License to anyone who comes into possession of a copy. This - License will therefore apply, along with any applicable section 7 - additional terms, to the whole of the work, and all its parts, - regardless of how they are packaged. This License gives no - permission to license the work in any other way, but it does not - invalidate such permission if you have separately received it. - - d) If the work has interactive user interfaces, each must display - Appropriate Legal Notices; however, if the Program has interactive - interfaces that do not display Appropriate Legal Notices, your - work need not make them do so. - - A compilation of a covered work with other separate and independent -works, which are not by their nature extensions of the covered work, -and which are not combined with it such as to form a larger program, -in or on a volume of a storage or distribution medium, is called an -"aggregate" if the compilation and its resulting copyright are not -used to limit the access or legal rights of the compilation's users -beyond what the individual works permit. Inclusion of a covered work -in an aggregate does not cause this License to apply to the other -parts of the aggregate. - - 6. Conveying Non-Source Forms. - - You may convey a covered work in object code form under the terms -of sections 4 and 5, provided that you also convey the -machine-readable Corresponding Source under the terms of this License, -in one of these ways: - - a) Convey the object code in, or embodied in, a physical product - (including a physical distribution medium), accompanied by the - Corresponding Source fixed on a durable physical medium - customarily used for software interchange. - - b) Convey the object code in, or embodied in, a physical product - (including a physical distribution medium), accompanied by a - written offer, valid for at least three years and valid for as - long as you offer spare parts or customer support for that product - model, to give anyone who possesses the object code either (1) a - copy of the Corresponding Source for all the software in the - product that is covered by this License, on a durable physical - medium customarily used for software interchange, for a price no - more than your reasonable cost of physically performing this - conveying of source, or (2) access to copy the - Corresponding Source from a network server at no charge. - - c) Convey individual copies of the object code with a copy of the - written offer to provide the Corresponding Source. This - alternative is allowed only occasionally and noncommercially, and - only if you received the object code with such an offer, in accord - with subsection 6b. - - d) Convey the object code by offering access from a designated - place (gratis or for a charge), and offer equivalent access to the - Corresponding Source in the same way through the same place at no - further charge. You need not require recipients to copy the - Corresponding Source along with the object code. If the place to - copy the object code is a network server, the Corresponding Source - may be on a different server (operated by you or a third party) - that supports equivalent copying facilities, provided you maintain - clear directions next to the object code saying where to find the - Corresponding Source. Regardless of what server hosts the - Corresponding Source, you remain obligated to ensure that it is - available for as long as needed to satisfy these requirements. - - e) Convey the object code using peer-to-peer transmission, provided - you inform other peers where the object code and Corresponding - Source of the work are being offered to the general public at no - charge under subsection 6d. - - A separable portion of the object code, whose source code is excluded -from the Corresponding Source as a System Library, need not be -included in conveying the object code work. - - A "User Product" is either (1) a "consumer product", which means any -tangible personal property which is normally used for personal, family, -or household purposes, or (2) anything designed or sold for incorporation -into a dwelling. In determining whether a product is a consumer product, -doubtful cases shall be resolved in favor of coverage. For a particular -product received by a particular user, "normally used" refers to a -typical or common use of that class of product, regardless of the status -of the particular user or of the way in which the particular user -actually uses, or expects or is expected to use, the product. A product -is a consumer product regardless of whether the product has substantial -commercial, industrial or non-consumer uses, unless such uses represent -the only significant mode of use of the product. - - "Installation Information" for a User Product means any methods, -procedures, authorization keys, or other information required to install -and execute modified versions of a covered work in that User Product from -a modified version of its Corresponding Source. The information must -suffice to ensure that the continued functioning of the modified object -code is in no case prevented or interfered with solely because -modification has been made. - - If you convey an object code work under this section in, or with, or -specifically for use in, a User Product, and the conveying occurs as -part of a transaction in which the right of possession and use of the -User Product is transferred to the recipient in perpetuity or for a -fixed term (regardless of how the transaction is characterized), the -Corresponding Source conveyed under this section must be accompanied -by the Installation Information. But this requirement does not apply -if neither you nor any third party retains the ability to install -modified object code on the User Product (for example, the work has -been installed in ROM). - - The requirement to provide Installation Information does not include a -requirement to continue to provide support service, warranty, or updates -for a work that has been modified or installed by the recipient, or for -the User Product in which it has been modified or installed. Access to a -network may be denied when the modification itself materially and -adversely affects the operation of the network or violates the rules and -protocols for communication across the network. - - Corresponding Source conveyed, and Installation Information provided, -in accord with this section must be in a format that is publicly -documented (and with an implementation available to the public in -source code form), and must require no special password or key for -unpacking, reading or copying. - - 7. Additional Terms. - - "Additional permissions" are terms that supplement the terms of this -License by making exceptions from one or more of its conditions. -Additional permissions that are applicable to the entire Program shall -be treated as though they were included in this License, to the extent -that they are valid under applicable law. If additional permissions -apply only to part of the Program, that part may be used separately -under those permissions, but the entire Program remains governed by -this License without regard to the additional permissions. - - When you convey a copy of a covered work, you may at your option -remove any additional permissions from that copy, or from any part of -it. (Additional permissions may be written to require their own -removal in certain cases when you modify the work.) You may place -additional permissions on material, added by you to a covered work, -for which you have or can give appropriate copyright permission. - - Notwithstanding any other provision of this License, for material you -add to a covered work, you may (if authorized by the copyright holders of -that material) supplement the terms of this License with terms: - - a) Disclaiming warranty or limiting liability differently from the - terms of sections 15 and 16 of this License; or - - b) Requiring preservation of specified reasonable legal notices or - author attributions in that material or in the Appropriate Legal - Notices displayed by works containing it; or - - c) Prohibiting misrepresentation of the origin of that material, or - requiring that modified versions of such material be marked in - reasonable ways as different from the original version; or - - d) Limiting the use for publicity purposes of names of licensors or - authors of the material; or - - e) Declining to grant rights under trademark law for use of some - trade names, trademarks, or service marks; or - - f) Requiring indemnification of licensors and authors of that - material by anyone who conveys the material (or modified versions of - it) with contractual assumptions of liability to the recipient, for - any liability that these contractual assumptions directly impose on - those licensors and authors. - - All other non-permissive additional terms are considered "further -restrictions" within the meaning of section 10. If the Program as you -received it, or any part of it, contains a notice stating that it is -governed by this License along with a term that is a further -restriction, you may remove that term. If a license document contains -a further restriction but permits relicensing or conveying under this -License, you may add to a covered work material governed by the terms -of that license document, provided that the further restriction does -not survive such relicensing or conveying. - - If you add terms to a covered work in accord with this section, you -must place, in the relevant source files, a statement of the -additional terms that apply to those files, or a notice indicating -where to find the applicable terms. - - Additional terms, permissive or non-permissive, may be stated in the -form of a separately written license, or stated as exceptions; -the above requirements apply either way. - - 8. Termination. - - You may not propagate or modify a covered work except as expressly -provided under this License. Any attempt otherwise to propagate or -modify it is void, and will automatically terminate your rights under -this License (including any patent licenses granted under the third -paragraph of section 11). - - However, if you cease all violation of this License, then your -license from a particular copyright holder is reinstated (a) -provisionally, unless and until the copyright holder explicitly and -finally terminates your license, and (b) permanently, if the copyright -holder fails to notify you of the violation by some reasonable means -prior to 60 days after the cessation. - - Moreover, your license from a particular copyright holder is -reinstated permanently if the copyright holder notifies you of the -violation by some reasonable means, this is the first time you have -received notice of violation of this License (for any work) from that -copyright holder, and you cure the violation prior to 30 days after -your receipt of the notice. - - Termination of your rights under this section does not terminate the -licenses of parties who have received copies or rights from you under -this License. If your rights have been terminated and not permanently -reinstated, you do not qualify to receive new licenses for the same -material under section 10. - - 9. Acceptance Not Required for Having Copies. - - You are not required to accept this License in order to receive or -run a copy of the Program. Ancillary propagation of a covered work -occurring solely as a consequence of using peer-to-peer transmission -to receive a copy likewise does not require acceptance. However, -nothing other than this License grants you permission to propagate or -modify any covered work. These actions infringe copyright if you do -not accept this License. Therefore, by modifying or propagating a -covered work, you indicate your acceptance of this License to do so. - - 10. Automatic Licensing of Downstream Recipients. - - Each time you convey a covered work, the recipient automatically -receives a license from the original licensors, to run, modify and -propagate that work, subject to this License. You are not responsible -for enforcing compliance by third parties with this License. - - An "entity transaction" is a transaction transferring control of an -organization, or substantially all assets of one, or subdividing an -organization, or merging organizations. If propagation of a covered -work results from an entity transaction, each party to that -transaction who receives a copy of the work also receives whatever -licenses to the work the party's predecessor in interest had or could -give under the previous paragraph, plus a right to possession of the -Corresponding Source of the work from the predecessor in interest, if -the predecessor has it or can get it with reasonable efforts. - - You may not impose any further restrictions on the exercise of the -rights granted or affirmed under this License. For example, you may -not impose a license fee, royalty, or other charge for exercise of -rights granted under this License, and you may not initiate litigation -(including a cross-claim or counterclaim in a lawsuit) alleging that -any patent claim is infringed by making, using, selling, offering for -sale, or importing the Program or any portion of it. - - 11. Patents. - - A "contributor" is a copyright holder who authorizes use under this -License of the Program or a work on which the Program is based. The -work thus licensed is called the contributor's "contributor version". - - A contributor's "essential patent claims" are all patent claims -owned or controlled by the contributor, whether already acquired or -hereafter acquired, that would be infringed by some manner, permitted -by this License, of making, using, or selling its contributor version, -but do not include claims that would be infringed only as a -consequence of further modification of the contributor version. For -purposes of this definition, "control" includes the right to grant -patent sublicenses in a manner consistent with the requirements of -this License. - - Each contributor grants you a non-exclusive, worldwide, royalty-free -patent license under the contributor's essential patent claims, to -make, use, sell, offer for sale, import and otherwise run, modify and -propagate the contents of its contributor version. - - In the following three paragraphs, a "patent license" is any express -agreement or commitment, however denominated, not to enforce a patent -(such as an express permission to practice a patent or covenant not to -sue for patent infringement). To "grant" such a patent license to a -party means to make such an agreement or commitment not to enforce a -patent against the party. - - If you convey a covered work, knowingly relying on a patent license, -and the Corresponding Source of the work is not available for anyone -to copy, free of charge and under the terms of this License, through a -publicly available network server or other readily accessible means, -then you must either (1) cause the Corresponding Source to be so -available, or (2) arrange to deprive yourself of the benefit of the -patent license for this particular work, or (3) arrange, in a manner -consistent with the requirements of this License, to extend the patent -license to downstream recipients. "Knowingly relying" means you have -actual knowledge that, but for the patent license, your conveying the -covered work in a country, or your recipient's use of the covered work -in a country, would infringe one or more identifiable patents in that -country that you have reason to believe are valid. - - If, pursuant to or in connection with a single transaction or -arrangement, you convey, or propagate by procuring conveyance of, a -covered work, and grant a patent license to some of the parties -receiving the covered work authorizing them to use, propagate, modify -or convey a specific copy of the covered work, then the patent license -you grant is automatically extended to all recipients of the covered -work and works based on it. - - A patent license is "discriminatory" if it does not include within -the scope of its coverage, prohibits the exercise of, or is -conditioned on the non-exercise of one or more of the rights that are -specifically granted under this License. You may not convey a covered -work if you are a party to an arrangement with a third party that is -in the business of distributing software, under which you make payment -to the third party based on the extent of your activity of conveying -the work, and under which the third party grants, to any of the -parties who would receive the covered work from you, a discriminatory -patent license (a) in connection with copies of the covered work -conveyed by you (or copies made from those copies), or (b) primarily -for and in connection with specific products or compilations that -contain the covered work, unless you entered into that arrangement, -or that patent license was granted, prior to 28 March 2007. - - Nothing in this License shall be construed as excluding or limiting -any implied license or other defenses to infringement that may -otherwise be available to you under applicable patent law. - - 12. No Surrender of Others' Freedom. - - If conditions are imposed on you (whether by court order, agreement or -otherwise) that contradict the conditions of this License, they do not -excuse you from the conditions of this License. If you cannot convey a -covered work so as to satisfy simultaneously your obligations under this -License and any other pertinent obligations, then as a consequence you may -not convey it at all. For example, if you agree to terms that obligate you -to collect a royalty for further conveying from those to whom you convey -the Program, the only way you could satisfy both those terms and this -License would be to refrain entirely from conveying the Program. - - 13. Use with the GNU Affero General Public License. - - Notwithstanding any other provision of this License, you have -permission to link or combine any covered work with a work licensed -under version 3 of the GNU Affero General Public License into a single -combined work, and to convey the resulting work. The terms of this -License will continue to apply to the part which is the covered work, -but the special requirements of the GNU Affero General Public License, -section 13, concerning interaction through a network will apply to the -combination as such. - - 14. Revised Versions of this License. - - The Free Software Foundation may publish revised and/or new versions of -the GNU General Public License from time to time. Such new versions will -be similar in spirit to the present version, but may differ in detail to -address new problems or concerns. - - Each version is given a distinguishing version number. If the -Program specifies that a certain numbered version of the GNU General -Public License "or any later version" applies to it, you have the -option of following the terms and conditions either of that numbered -version or of any later version published by the Free Software -Foundation. If the Program does not specify a version number of the -GNU General Public License, you may choose any version ever published -by the Free Software Foundation. - - If the Program specifies that a proxy can decide which future -versions of the GNU General Public License can be used, that proxy's -public statement of acceptance of a version permanently authorizes you -to choose that version for the Program. - - Later license versions may give you additional or different -permissions. However, no additional obligations are imposed on any -author or copyright holder as a result of your choosing to follow a -later version. - - 15. Disclaimer of Warranty. - - THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY -APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT -HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY -OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, -THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR -PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM -IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF -ALL NECESSARY SERVICING, REPAIR OR CORRECTION. - - 16. Limitation of Liability. - - IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING -WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS -THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY -GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE -USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF -DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD -PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), -EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF -SUCH DAMAGES. - - 17. Interpretation of Sections 15 and 16. - - If the disclaimer of warranty and limitation of liability provided -above cannot be given local legal effect according to their terms, -reviewing courts shall apply local law that most closely approximates -an absolute waiver of all civil liability in connection with the -Program, unless a warranty or assumption of liability accompanies a -copy of the Program in return for a fee. - - END OF TERMS AND CONDITIONS - - How to Apply These Terms to Your New Programs - - If you develop a new program, and you want it to be of the greatest -possible use to the public, the best way to achieve this is to make it -free software which everyone can redistribute and change under these terms. - - To do so, attach the following notices to the program. It is safest -to attach them to the start of each source file to most effectively -state the exclusion of warranty; and each file should have at least -the "copyright" line and a pointer to where the full notice is found. - - - Copyright (C) - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - -Also add information on how to contact you by electronic and paper mail. - - If the program does terminal interaction, make it output a short -notice like this when it starts in an interactive mode: - - Copyright (C) - This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. - This is free software, and you are welcome to redistribute it - under certain conditions; type `show c' for details. - -The hypothetical commands `show w' and `show c' should show the appropriate -parts of the General Public License. Of course, your program's commands -might be different; for a GUI interface, you would use an "about box". - - You should also get your employer (if you work as a programmer) or school, -if any, to sign a "copyright disclaimer" for the program, if necessary. -For more information on this, and how to apply and follow the GNU GPL, see -. - - The GNU General Public License does not permit incorporating your program -into proprietary programs. If your program is a subroutine library, you -may consider it more useful to permit linking proprietary applications with -the library. If this is what you want to do, use the GNU Lesser General -Public License instead of this License. But first, please read -. diff --git a/mola_navstate_fg/README.md b/mola_navstate_fg/README.md deleted file mode 100644 index 25cb2e85..00000000 --- a/mola_navstate_fg/README.md +++ /dev/null @@ -1,14 +0,0 @@ -# mola_navstate_fuse -Sliding window Factor-graph data fusion for odometry, IMU, GNSS, and SE(3) pose/twist estimations. - -This repository provides: -* `NavStateFuse`: C++ class to integrate odometry, IMU, and pose/twist estimations. - -See package [documentation](https://docs.mola-slam.org/latest/modules.html). - - -## Build and install -Refer to the [root MOLA repository](https://github.com/MOLAorg/mola). - -## License -This package is released under the GNU GPL v3 license. Other options available upon request. diff --git a/mola_navstate_fg/apps/mola-navstate-cli.cpp b/mola_navstate_fg/apps/mola-navstate-cli.cpp deleted file mode 100644 index 3e811d29..00000000 --- a/mola_navstate_fg/apps/mola-navstate-cli.cpp +++ /dev/null @@ -1,93 +0,0 @@ -/* ------------------------------------------------------------------------- - * A Modular Optimization framework for Localization and mApping (MOLA) - * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria - * See LICENSE for license information. - * ------------------------------------------------------------------------- */ - -#include -#include -#include -#include -#include -#include - -#include - -namespace -{ -void run_navstate(const std::string& paramsFile, const std::string& rawlogFile) -{ - using mrpt::obs::CObservationGPS; - using mrpt::obs::CObservationIMU; - using mrpt::obs::CObservationRobotPose; - - mola::NavStateFG nav; - - std::cout << "Initalizing from: " << paramsFile << std::endl; - - const auto cfg = mrpt::containers::yaml::FromFile(paramsFile); - nav.initialize(cfg); - - std::cout << "Reading dataset from: " << rawlogFile << std::endl; - - mrpt::obs::CRawlog dataset; - dataset.loadFromRawLogFile(rawlogFile); - - const std::string frame_id = "map"; - - std::cout << "Read entries: " << dataset.size() << std::endl; - - for (size_t i = 0; i < dataset.size(); i++) - { - const auto o = dataset.getAsObservation(i); - if (!o) continue; - - if (auto oGPS = std::dynamic_pointer_cast(o); oGPS) - { - nav.fuse_gnss(*oGPS); - } - else if (auto oPose = - std::dynamic_pointer_cast(o); - oPose) - { - nav.fuse_pose(oPose->timestamp, oPose->pose, frame_id); - } - else if (auto oImu = std::dynamic_pointer_cast(o); - oImu) - { - nav.fuse_imu(*oImu); - } - else - { - std::cout << "[Warning] Ignoring observation #" << i << ": '" - << o->sensorLabel - << "' of type: " << o->GetRuntimeClass()->className - << "\n"; - } - - } // for each entry -} - -} // namespace - -int main(int argc, char** argv) -{ - try - { - if (argc != 3) - { - std::cerr << "Usage: " << argv[0] << " params.yaml dataset.rawlog" - << std::endl; - return 1; - } - - run_navstate(argv[1], argv[2]); - - return 0; - } - catch (const std::exception& e) - { - std::cerr << "Exception: " << e.what() << std::endl; - return 1; - } -} diff --git a/mola_navstate_fg/docs/module-mola-navstate-fg.rst b/mola_navstate_fg/docs/module-mola-navstate-fg.rst deleted file mode 100644 index f4ae4afc..00000000 --- a/mola_navstate_fg/docs/module-mola-navstate-fg.rst +++ /dev/null @@ -1,12 +0,0 @@ -.. _mola-navstate-fg: - -======================================== -Module: mola-navstate-fg -======================================== - -Write me! - - -.. index:: - single: mola-navstate-fg - module: mola-navstate-fg diff --git a/mola_navstate_fg/include/mola_navstate_fg/NavStateFG.h b/mola_navstate_fg/include/mola_navstate_fg/NavStateFG.h deleted file mode 100644 index 14a958c1..00000000 --- a/mola_navstate_fg/include/mola_navstate_fg/NavStateFG.h +++ /dev/null @@ -1,243 +0,0 @@ -/* ------------------------------------------------------------------------- - * A Modular Optimization framework for Localization and mApping (MOLA) - * - * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria - * Licensed under the GNU GPL v3 for non-commercial applications. - * - * This file is part of MOLA. - * MOLA is free software: you can redistribute it and/or modify it under the - * terms of the GNU General Public License as published by the Free Software - * Foundation, either version 3 of the License, or (at your option) any later - * version. - * - * MOLA is distributed in the hope that it will be useful, but WITHOUT ANY - * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR - * A PARTICULAR PURPOSE. See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along with - * MOLA. If not, see . - * ------------------------------------------------------------------------- */ -/** - * @file NavStateFG.h - * @brief Fuse of odometry, IMU, and SE(3) pose/twist estimations. - * @author Jose Luis Blanco Claraco - * @date Jan 22, 2024 - */ -#pragma once - -// this package: -#include -#include - -// MOLA: -#include -#include - -// MRPT: -#include -#include -#include -#include -#include -#include -#include - -// std: -#include -#include - -namespace mola -{ -/** Sliding window Factor-graph data fusion for odometry, IMU, GNSS, and SE(3) - * pose/twist estimations. - * - * Frame conventions: - * - There is a frame of reference for each source of odometry, e.g. - * there may be one for LiDAR-odometry, another for visual-odometry, or - * wheels-based odometry, etc. Each such frame is referenced with a "frame - * name" (an arbitrary string). - * - Internally, the first frame of reference will be used as "global" - * coordinates, despite it may be actually either a `map` or `odom` frame, in - * the [ROS REP 105](https://www.ros.org/reps/rep-0105.html) sense. - * - IMU readings are, by definition, given in the robot body frame, although - * they can have a relative transformation between the vehicle and sensor. - * - * Main API methods and frame conventions: - * - `estimated_navstate()`: Output estimations can be requested in any of the - * existing frames of reference. - * - `fuse_pose()`: Can be used to integrate information from any "odometry" or - * "localization" input, as mentioned above. - * - `fuse_gnss()`: TO-DO. - * - `fuse_imu()`: TO-DO. - * - * Usage: - * - (1) Call initialize() or set the required parameters directly in params_. - * - (2) Integrate measurements with `fuse_*()` methods. Each CObservation - * class includes a `timestamp` field which is used to estimate the - * trajectory. - * - (3) Repeat (2) as needed. - * - (4) Read the estimation up to any nearby moment in time with - * estimated_navstate() - * - * Old observations are automatically removed. - * - * A constant SE(3) velocity model is internally used, without any - * particular assumptions on the vehicle kinematics. - * - * For more theoretical descriptions, see the papers cited in - * https://docs.mola-slam.org/latest/ - * - * \ingroup mola_navstate_fuse_grp - */ -class NavStateFG : public mola::NavStateFilter -{ - public: - NavStateFG(); - ~NavStateFG(); - - /** \name Main API - * @{ */ - - NavStateFGParams params_; - - /** - * @brief Initializes the object and reads all parameters from a YAML node. - * @param cfg a YAML node with a dictionary of parameters to load from. - */ - void initialize(const mrpt::containers::yaml& cfg) override; - - /** Resets the estimator state to an initial state */ - void reset() override; - - /** Integrates new SE(3) pose estimation of the vehicle wrt frame_id - */ - void fuse_pose( - const mrpt::Clock::time_point& timestamp, - const mrpt::poses::CPose3DPDFGaussian& pose, - const std::string& frame_id) override; - - /** Integrates new wheels-based odometry observations into the estimator. - * This is a convenience method that internally ends up calling - * fuse_pose(), but computing the uncertainty of odometry increments - * according to a given motion model. - */ - void fuse_odometry( - const mrpt::obs::CObservationOdometry& odom, - const std::string& odomName = "odom_wheels") override; - - /** Integrates new IMU observations into the estimator */ - void fuse_imu(const mrpt::obs::CObservationIMU& imu) override; - - /** Integrates new GNSS observations into the estimator */ - void fuse_gnss(const mrpt::obs::CObservationGPS& gps) override; - - /** Integrates new twist estimation (in the odom frame) */ - void fuse_twist( - const mrpt::Clock::time_point& timestamp, - const mrpt::math::TTwist3D& twist, - const mrpt::math::CMatrixDouble66& twistCov) override; - - /** Computes the estimated vehicle state at a given timestep using the - * observations in the time window. A std::nullopt is returned if there is - * no valid observations yet, or if requested a timestamp out of the model - * validity time window (e.g. too far in the future to be trustful). - */ - std::optional estimated_navstate( - const mrpt::Clock::time_point& timestamp, - const std::string& frame_id) override; - - /// Returns a list of known frame_ids: - auto known_frame_ids() -> std::set; - - /** @} */ - - private: - // everything related to gtsam is hidden in the public API via pimpl - struct GtsamImpl; - - using frameid_t = uint8_t; - - // an observation from fuse_pose() - struct PoseData - { - PoseData() = default; - - mrpt::poses::CPose3DPDFGaussian pose; - frameid_t frameId; - }; - - // an observation from fuse_odometry() - struct OdomData - { - OdomData() = default; - - mrpt::poses::CPose3D pose; - frameid_t frameId; - }; - - // an observation from fuse_twist() - struct TwistData - { - TwistData() = default; - mrpt::math::TTwist3D twist; // in the local frame of reference - mrpt::math::CMatrixDouble66 twistCov; - }; - - // Dummy type representing the query point. - struct QueryPointData - { - QueryPointData() = default; - }; - - struct PointData - { - PointData() = default; - - PointData(const PoseData& p) : pose(p) {} - PointData(const OdomData& p) : odom(p) {} - PointData(const TwistData& p) : twist(p) {} - PointData(const QueryPointData& p) : query(p) {} - - std::optional pose; - std::optional odom; - std::optional twist; - std::optional query; - - std::string asString() const; - - bool empty() const { return !pose && !odom && !twist && !query; } - }; - - struct State - { - State(); - ~State(); - - mrpt::pimpl impl; - - /// A bimap of known "frame_id" <=> "numeric IDs": - mrpt::containers::bimap known_frames; - - /// Returns the existing ID, or creates a new ID, for a frame: - frameid_t frame_id(const std::string& frame_name); - - /// The sliding window of observation data: - std::map data; - - auto last_pose_of_frame_id(const std::string& frame_id) - -> std::optional>; - }; - - State state_; - - std::optional build_and_optimize_fg( - const mrpt::Clock::time_point queryTimestamp, - const std::string& frame_id); - - /// Implementation of Eqs (1),(4) in the MOLA RSS2019 paper. - void addFactor(const mola::FactorConstVelKinematics& f); - - void delete_too_old_entries(); -}; - -} // namespace mola diff --git a/mola_navstate_fg/include/mola_navstate_fg/NavStateFGParams.h b/mola_navstate_fg/include/mola_navstate_fg/NavStateFGParams.h deleted file mode 100644 index 9d017252..00000000 --- a/mola_navstate_fg/include/mola_navstate_fg/NavStateFGParams.h +++ /dev/null @@ -1,74 +0,0 @@ -/* ------------------------------------------------------------------------- - * A Modular Optimization framework for Localization and mApping (MOLA) - * - * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria - * Licensed under the GNU GPL v3 for non-commercial applications. - * - * This file is part of MOLA. - * MOLA is free software: you can redistribute it and/or modify it under the - * terms of the GNU General Public License as published by the Free Software - * Foundation, either version 3 of the License, or (at your option) any later - * version. - * - * MOLA is distributed in the hope that it will be useful, but WITHOUT ANY - * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR - * A PARTICULAR PURPOSE. See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along with - * MOLA. If not, see . - * ------------------------------------------------------------------------- */ -/** - * @file NavStateFGParams.h - * @brief Parameters for NavStateFG - * @author Jose Luis Blanco Claraco - * @date Jan 22, 2024 - */ - -#pragma once - -#include -#include -#include - -namespace mola -{ -/** Parameters needed by NavStateFG. - * - * \ingroup mola_navstate_fuse__grp - */ -class NavStateFGParams -{ - public: - NavStateFGParams() = default; - ~NavStateFGParams() = default; - - /// Loads all parameters from a YAML map node. - void loadFrom(const mrpt::containers::yaml& cfg); - - /** Valid estimations will be extrapolated only up to this time since the - * last incorporated observation. If a request is done farther away, an - * empty estimation will be returned. - */ - double max_time_to_use_velocity_model = 2.0; // [s] - - /// Time to keep past observations in the filter - double sliding_window_length = 5.0; // [s] - - /// If the time between two keyframes is larger than this, a warning will be - /// emitted; but the algorithm will keep trying its best. - double time_between_frames_to_warning = 3.0; // [s] - - double sigma_random_walk_acceleration_linear = 1.0; // [m/s²] - double sigma_random_walk_acceleration_angular = 1.0; // [rad/s²] - double sigma_integrator_position = 0.10; // [m] - double sigma_integrator_orientation = 0.10; // [rad] - - double robust_param = 0.0; // 0: no robust - double max_rmse = 2.0; - - mrpt::math::TTwist3D initial_twist; - double initial_twist_sigma_lin = 20.0; // [m/s] - double initial_twist_sigma_ang = 3.0; // [rad/s] -}; - -} // namespace mola diff --git a/mola_navstate_fg/package.xml b/mola_navstate_fg/package.xml deleted file mode 100644 index 7fb85ab5..00000000 --- a/mola_navstate_fg/package.xml +++ /dev/null @@ -1,45 +0,0 @@ - - - - - mola_navstate_fg - 1.4.1 - SE(3) pose and twist path data fusion estimator - - Jose-Luis Blanco-Claraco - GPLv3 - - https://github.com/MOLAorg/mola/tree/develop/mola_navstate_fuse - - - mola_common - mola_kernel - mola_imu_preintegration - - mrpt_libobs - - - gtsam - libboost-serialization-dev - libboost-system-dev - libboost-filesystem-dev - libboost-thread-dev - libboost-program-options-dev - libboost-date-time-dev - libboost-timer-dev - libboost-chrono-dev - libboost-regex-dev - - - doxygen - - - cmake - - cmake - - - - diff --git a/mola_navstate_fg/src/FactorAngularVelocityIntegration.h b/mola_navstate_fg/src/FactorAngularVelocityIntegration.h deleted file mode 100644 index 8a6f4c11..00000000 --- a/mola_navstate_fg/src/FactorAngularVelocityIntegration.h +++ /dev/null @@ -1,136 +0,0 @@ -/* ------------------------------------------------------------------------- - * A Modular Optimization framework for Localization and mApping (MOLA) - * - * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria - * Licensed under the GNU GPL v3 for non-commercial applications. - * - * This file is part of MOLA. - * MOLA is free software: you can redistribute it and/or modify it under the - * terms of the GNU General Public License as published by the Free Software - * Foundation, either version 3 of the License, or (at your option) any later - * version. - * - * MOLA is distributed in the hope that it will be useful, but WITHOUT ANY - * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR - * A PARTICULAR PURPOSE. See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along with - * MOLA. If not, see . - * ------------------------------------------------------------------------- */ -/** - * @file FactorAngularVelocityIntegration.h - * @brief GTSAM factor - * @author Jose Luis Blanco Claraco - * @date Jun 13, 2024 - */ - -#pragma once - -#include -#include -#include -#include -#include - -namespace mola -{ -/** - * Factor for angular velocity integration model, equivalent to expressions: - * - * gtsam::Expression deltaWi = dt * bWi; - * gtsam::Expression expmap_(>sam::Rot3::Expmap, deltaWi); - * - * gtsam::between( gtsam::compose(Ri, expmap_), Rj ) = Rot_Identity - * - */ -class FactorAngularVelocityIntegration - : public gtsam::ExpressionFactorN< - gtsam::Rot3 /*return type*/, gtsam::Rot3 /* Ri */, - gtsam::Point3 /* bWi */, gtsam::Rot3 /* Rj */ - > -{ - private: - using This = FactorAngularVelocityIntegration; - using Base = gtsam::ExpressionFactorN< - gtsam::Rot3 /*return type*/, gtsam::Rot3 /* Ri */, - gtsam::Point3 /* bWi */, gtsam::Rot3 /* Rj */ - >; - - double dt_ = .0; - - public: - /// default constructor - FactorAngularVelocityIntegration() = default; - ~FactorAngularVelocityIntegration() override = default; - - FactorAngularVelocityIntegration( - gtsam::Key kRi, gtsam::Key kbWi, gtsam::Key kRj, const double dt, - const gtsam::SharedNoiseModel& model) - : Base({kRi, kbWi, kRj}, model, gtsam::Rot3()), dt_(dt) - { - this->initialize(This::expression({kRi, kbWi, kRj})); - } - - /// @return a deep copy of this factor - gtsam::NonlinearFactor::shared_ptr clone() const override - { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); - } - - // Return measurement expression - gtsam::Expression expression( - const std::array& keys) const override - { - gtsam::Rot3_ Ri(keys[0]); - gtsam::Point3_ bWi(keys[1]); - gtsam::Rot3_ Rj(keys[2]); - gtsam::Expression deltaWi = dt_ * bWi; - gtsam::Expression expmap_(>sam::Rot3::Expmap, deltaWi); - - return gtsam::between(gtsam::compose(Ri, expmap_), Rj); - } - - /** implement functions needed for Testable */ - - /** print */ - void print( - const std::string& s, const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const override - { - std::cout << s << "FactorAngularVelocityIntegration(" - << keyFormatter(Factor::keys_[0]) << "," - << keyFormatter(Factor::keys_[1]) << "," - << keyFormatter(Factor::keys_[2]) << ")\n"; - gtsam::traits::Print(measured_, " measured: "); - gtsam::traits::Print(dt_, " dt: "); - this->noiseModel_->print(" noise model: "); - } - - /** equals */ - bool equals(const gtsam::NonlinearFactor& expected, double tol = 1e-9) - const override - { - const This* e = dynamic_cast(&expected); - return e != nullptr && Base::equals(*e, tol) && - gtsam::traits::Equals(dt_, e->dt_, tol); - } - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE& ar, const unsigned int /*version*/) - { - // **IMPORTANT** We need to deserialize parameters before the base - // class, since it calls expression() and we need all parameters ready - // at that point. - ar& BOOST_SERIALIZATION_NVP(measured_); - ar& BOOST_SERIALIZATION_NVP(dt_); - ar& boost::serialization::make_nvp( - "FactorAngularVelocityIntegration", - boost::serialization::base_object(*this)); - } -}; - -} // namespace mola diff --git a/mola_navstate_fg/src/FactorConstAngularVelocity.h b/mola_navstate_fg/src/FactorConstAngularVelocity.h deleted file mode 100644 index 98a00fc0..00000000 --- a/mola_navstate_fg/src/FactorConstAngularVelocity.h +++ /dev/null @@ -1,129 +0,0 @@ -/* ------------------------------------------------------------------------- - * A Modular Optimization framework for Localization and mApping (MOLA) - * - * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria - * Licensed under the GNU GPL v3 for non-commercial applications. - * - * This file is part of MOLA. - * MOLA is free software: you can redistribute it and/or modify it under the - * terms of the GNU General Public License as published by the Free Software - * Foundation, either version 3 of the License, or (at your option) any later - * version. - * - * MOLA is distributed in the hope that it will be useful, but WITHOUT ANY - * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR - * A PARTICULAR PURPOSE. See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along with - * MOLA. If not, see . - * ------------------------------------------------------------------------- */ -/** - * @file FactorAngularVelocityIntegration.h - * @brief GTSAM factor - * @author Jose Luis Blanco Claraco - * @date Jun 13, 2024 - */ - -#pragma once - -#include -#include -#include -#include -#include - -namespace mola -{ -/** - * Factor for constant angular velocity model, equivalent to expression: - * - * gtsam::rotate(Ri, bWi) - gtsam::rotate(Rj, bWj) = errZero - * - */ -class FactorConstAngularVelocity - : public gtsam::ExpressionFactorN< - gtsam::Point3 /*return type*/, gtsam::Rot3, gtsam::Point3, - gtsam::Rot3, gtsam::Point3> -{ - private: - using This = FactorConstAngularVelocity; - using Base = gtsam::ExpressionFactorN< - gtsam::Point3, gtsam::Rot3, gtsam::Point3, gtsam::Rot3, gtsam::Point3>; - - public: - /// default constructor - FactorConstAngularVelocity() = default; - ~FactorConstAngularVelocity() override = default; - - FactorConstAngularVelocity( - gtsam::Key kRi, gtsam::Key kWi, gtsam::Key kRj, gtsam::Key kWj, - const gtsam::SharedNoiseModel& model) - : Base({kRi, kWi, kRj, kWj}, model, {0, 0, 0}) - { - this->initialize(This::expression({kRi, kWi, kRj, kWj})); - } - - /// @return a deep copy of this factor - gtsam::NonlinearFactor::shared_ptr clone() const override - { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); - } - - // Return measurement expression - gtsam::Expression expression( - const std::array& keys) const override - { - gtsam::Expression Ri_(keys[0]); - gtsam::Expression bWi_(keys[1]); - gtsam::Expression Rj_(keys[2]); - gtsam::Expression bWj_(keys[3]); - return {gtsam::rotate(Ri_, bWi_) - gtsam::rotate(Rj_, bWj_)}; - } - - /** implement functions needed for Testable */ - - /** print */ - void print( - const std::string& s, const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const override - { - std::cout << s << "FactorConstAngularVelocity(" - << keyFormatter(Factor::keys_[0]) << "," - << keyFormatter(Factor::keys_[1]) << "," - << keyFormatter(Factor::keys_[2]) << "," - << keyFormatter(Factor::keys_[3]) << ")\n"; - gtsam::traits::Print(measured_, " measured: "); - this->noiseModel_->print(" noise model: "); - } - - /** equals */ - bool equals(const gtsam::NonlinearFactor& expected, double tol = 1e-9) - const override - { - const This* e = dynamic_cast(&expected); - return e != nullptr && Base::equals(*e, tol); - } - - /** implement functions needed to derive from Factor */ - - /** number of variables attached to this factor */ - // std::size_t size() const; - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE& ar, const unsigned int /*version*/) - { - // **IMPORTANT** We need to deserialize parameters before the base - // class, since it calls expression() and we need all parameters ready - // at that point. - ar& BOOST_SERIALIZATION_NVP(measured_); - ar& boost::serialization::make_nvp( - "FactorConstAngularVelocity", - boost::serialization::base_object(*this)); - } -}; - -} // namespace mola diff --git a/mola_navstate_fg/src/FactorTrapezoidalIntegrator.h b/mola_navstate_fg/src/FactorTrapezoidalIntegrator.h deleted file mode 100644 index fb63911b..00000000 --- a/mola_navstate_fg/src/FactorTrapezoidalIntegrator.h +++ /dev/null @@ -1,142 +0,0 @@ -/* ------------------------------------------------------------------------- - * A Modular Optimization framework for Localization and mApping (MOLA) - * - * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria - * Licensed under the GNU GPL v3 for non-commercial applications. - * - * This file is part of MOLA. - * MOLA is free software: you can redistribute it and/or modify it under the - * terms of the GNU General Public License as published by the Free Software - * Foundation, either version 3 of the License, or (at your option) any later - * version. - * - * MOLA is distributed in the hope that it will be useful, but WITHOUT ANY - * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR - * A PARTICULAR PURPOSE. See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along with - * MOLA. If not, see . - * ------------------------------------------------------------------------- */ -/** - * @file FactorAngularVelocityIntegration.h - * @brief GTSAM factor - * @author Jose Luis Blanco Claraco - * @date Jun 13, 2024 - */ - -#pragma once - -#include -#include -#include -#include - -namespace mola -{ -/** - * Factor for constant angular velocity model, equivalent to expression: - * - * Pi + 0.5 * dt * (gtsam::rotate(Ri, bVi) + gtsam::rotate(Rj, bVj)) - Pj = - * errZero - * - */ -class FactorTrapezoidalIntegrator - : public gtsam::ExpressionFactorN< - gtsam::Point3 /*return type*/, // - gtsam::Point3, gtsam::Point3, gtsam::Rot3, // Pi, bVi, Ri - gtsam::Point3, gtsam::Point3, gtsam::Rot3> -{ - private: - using This = FactorTrapezoidalIntegrator; - using Base = gtsam::ExpressionFactorN< - gtsam::Point3 /*return type*/, gtsam::Point3, gtsam::Point3, - gtsam::Rot3, gtsam::Point3, gtsam::Point3, gtsam::Rot3>; - - double dt_ = .0; - - public: - /// default constructor - FactorTrapezoidalIntegrator() = default; - ~FactorTrapezoidalIntegrator() override = default; - - FactorTrapezoidalIntegrator( - gtsam::Key kPi, gtsam::Key kVi, gtsam::Key kRi, // - gtsam::Key kPj, gtsam::Key kVj, gtsam::Key kRj, // - const double dt, const gtsam::SharedNoiseModel& model) - : Base({kPi, kVi, kRi, kPj, kVj, kRj}, model, /* error=0 */ {0, 0, 0}), - dt_(dt) - { - this->initialize(This::expression({kPi, kVi, kRi, kPj, kVj, kRj})); - } - - /// @return a deep copy of this factor - gtsam::NonlinearFactor::shared_ptr clone() const override - { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); - } - - // Return measurement expression - gtsam::Expression expression( - const std::array& keys) const override - { - gtsam::Expression Pi_(keys[0]); - gtsam::Expression bVi_(keys[1]); - gtsam::Expression Ri_(keys[2]); - - gtsam::Expression Pj_(keys[3]); - gtsam::Expression bVj_(keys[4]); - gtsam::Expression Rj_(keys[5]); - - return { - Pi_ + - 0.5 * dt_ * (gtsam::rotate(Ri_, bVi_) + gtsam::rotate(Rj_, bVj_)) - - Pj_}; - } - - /** implement functions needed for Testable */ - - /** print */ - void print( - const std::string& s, const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const override - { - std::cout << s << "FactorTrapezoidalIntegrator(" - << keyFormatter(Factor::keys_[0]) << "," - << keyFormatter(Factor::keys_[1]) << "," - << keyFormatter(Factor::keys_[2]) << "," - << keyFormatter(Factor::keys_[3]) << "," - << keyFormatter(Factor::keys_[4]) << "," - << keyFormatter(Factor::keys_[5]) << ")\n"; - gtsam::traits::Print(dt_, " dt: "); - gtsam::traits::Print(measured_, " measured: "); - this->noiseModel_->print(" noise model: "); - } - - /** equals */ - bool equals(const gtsam::NonlinearFactor& expected, double tol = 1e-9) - const override - { - const This* e = dynamic_cast(&expected); - return e != nullptr && Base::equals(*e, tol) && - gtsam::traits::Equals(e->dt_, dt_, tol); - } - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE& ar, const unsigned int /*version*/) - { - // **IMPORTANT** We need to deserialize parameters before the base - // class, since it calls expression() and we need all parameters ready - // at that point. - ar& BOOST_SERIALIZATION_NVP(measured_); - ar& BOOST_SERIALIZATION_NVP(dt_); - ar& boost::serialization::make_nvp( - "FactorTrapezoidalIntegrator", - boost::serialization::base_object(*this)); - } -}; - -} // namespace mola diff --git a/mola_navstate_fg/src/NavStateFG.cpp b/mola_navstate_fg/src/NavStateFG.cpp deleted file mode 100644 index 4c7633f1..00000000 --- a/mola_navstate_fg/src/NavStateFG.cpp +++ /dev/null @@ -1,681 +0,0 @@ -/* ------------------------------------------------------------------------- - * A Modular Optimization framework for Localization and mApping (MOLA) - * - * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria - * Licensed under the GNU GPL v3 for non-commercial applications. - * - * This file is part of MOLA. - * MOLA is free software: you can redistribute it and/or modify it under the - * terms of the GNU General Public License as published by the Free Software - * Foundation, either version 3 of the License, or (at your option) any later - * version. - * - * MOLA is distributed in the hope that it will be useful, but WITHOUT ANY - * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR - * A PARTICULAR PURPOSE. See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along with - * MOLA. If not, see . - * ------------------------------------------------------------------------- */ -/** - * @file NavStateFG.cpp - * @brief Fuse of odometry, IMU, and SE(3) pose/twist estimations. - * @author Jose Luis Blanco Claraco - * @date Jan 22, 2024 - */ - -// MOLA & MRPT: -#include -#include -#include -#include -#include - -// GTSAM: -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// Custom factors: -#include "FactorAngularVelocityIntegration.h" -#include "FactorConstAngularVelocity.h" -#include "FactorTrapezoidalIntegrator.h" - -const bool NAVSTATE_PRINT_FG = mrpt::get_env("NAVSTATE_PRINT_FG", false); -const bool NAVSTATE_PRINT_FG_ERRORS = - mrpt::get_env("NAVSTATE_PRINT_FG_ERRORS", false); - -using namespace mola; - -using gtsam::symbol_shorthand::F; // Frame of reference origin pose (Pose3) -using gtsam::symbol_shorthand::P; // Position (Point3) -using gtsam::symbol_shorthand::R; // Rotation (Rot3) -using gtsam::symbol_shorthand::V; // Lin velocity (body frame) (Point3) -using gtsam::symbol_shorthand::W; // Ang velocity (body frame) (Point3) - -// -------- GtsamImpl ------- - -struct NavStateFG::GtsamImpl -{ - GtsamImpl() = default; - ~GtsamImpl() = default; - - gtsam::NonlinearFactorGraph fg; - gtsam::Values values; -}; - -// -------- NavStateFG::State ------- -NavStateFG::State::State() : impl(mrpt::make_impl()) {} -NavStateFG::State::~State() = default; - -NavStateFG::frameid_t NavStateFG::State::frame_id(const std::string& frame_name) -{ - if (auto it = known_frames.find_key(frame_name); - it != known_frames.getDirectMap().end()) - { - return it->second; - } - else - { - const auto newId = static_cast(known_frames.size()); - known_frames.insert(frame_name, newId); - return newId; - } -} - -std::optional> - NavStateFG::State::last_pose_of_frame_id(const std::string& frameId) -{ - const auto frId = frame_id(frameId); - for (auto it = data.rbegin(); it != data.rend(); ++it) - { - if (!it->second.pose) continue; - const auto& p = *it->second.pose; - if (p.frameId != frId) continue; - return std::make_pair(it->first, p); - } - return {}; -} - -// -------- NavStateFG ------- -NavStateFG::NavStateFG() -{ - this->mrpt::system::COutputLogger::setLoggerName("NavStateFG"); -} - -NavStateFG::~NavStateFG() = default; - -void NavStateFG::initialize(const mrpt::containers::yaml& cfg) -{ - reset(); - - // Load params: - params_.loadFrom(cfg); -} - -void NavStateFG::reset() -{ - // reset: - state_ = State(); -} - -void NavStateFG::fuse_odometry( - const mrpt::obs::CObservationOdometry& odom, const std::string& odomName) -{ - using namespace std::string_literals; - - THROW_EXCEPTION("finish implementation!"); - - ASSERT_(!odomName.empty()); - - OdomData d; - d.frameId = state_.frame_id(odomName); - d.pose = mrpt::poses::CPose3D(odom.odometry); - - state_.data.insert({odom.timestamp, d}); - - delete_too_old_entries(); -} - -void NavStateFG::fuse_imu(const mrpt::obs::CObservationIMU& imu) -{ - THROW_EXCEPTION("TODO"); - (void)imu; - - delete_too_old_entries(); -} - -void NavStateFG::fuse_gnss(const mrpt::obs::CObservationGPS& gps) -{ - THROW_EXCEPTION("TODO"); - (void)gps; - - delete_too_old_entries(); -} - -void NavStateFG::fuse_pose( - const mrpt::Clock::time_point& timestamp, - const mrpt::poses::CPose3DPDFGaussian& pose, const std::string& frame_id) -{ - // find last KF of this frame_id before adding the new one: - const auto lastKF = state_.last_pose_of_frame_id(frame_id); - - // numerical sanity: - for (int i = 0; i < 6; i++) ASSERT_GT_(pose.cov(i, i), .0); - - PoseData d; - d.frameId = state_.frame_id(frame_id); - d.pose = pose; - - state_.data.insert({timestamp, d}); - delete_too_old_entries(); - - // Estimate twist: - // If we add an additional direct observation of twist, the result is - // more accurate for coarser time steps: - if (!lastKF) return; - - const double dt = mrpt::system::timeDifference(lastKF->first, timestamp); - - if (dt > params_.max_time_to_use_velocity_model) return; - - ASSERT_GT_(dt, .0); - - mrpt::math::TTwist3D tw; - - const auto incrPosePdf = pose - lastKF->second.pose; - const auto incrPose = incrPosePdf.mean; - - tw.vx = incrPose.x() / dt; - tw.vy = incrPose.y() / dt; - tw.vz = incrPose.z() / dt; - - const auto logRot = - mrpt::poses::Lie::SO<3>::log(incrPose.getRotationMatrix()); - - tw.wx = logRot[0] / dt; - tw.wy = logRot[1] / dt; - tw.wz = logRot[2] / dt; - -#if 0 - // Estimate twist cov: - auto twCov = mrpt::math::CMatrixDouble66( - incrPosePdf.cov.asEigen() * (1.0 / (dt * dt))); -#endif - - // Ensure minimum covariance for avoiding overconfidence and numerical - // illness: - auto twCov = mrpt::math::CMatrixDouble66::Zero(); - for (int i = 0; i < 3; i++) - { - twCov(i, i) += mrpt::square(0.1); - twCov(3 + i, 3 + i) += mrpt::square(0.1); - } - - this->fuse_twist(timestamp, tw, twCov); -} - -void NavStateFG::fuse_twist( - const mrpt::Clock::time_point& timestamp, const mrpt::math::TTwist3D& twist, - const mrpt::math::CMatrixDouble66& twistCov) -{ - TwistData d; - d.twist = twist; - d.twistCov = twistCov; - - state_.data.insert({timestamp, d}); - - delete_too_old_entries(); -} - -std::optional NavStateFG::estimated_navstate( - const mrpt::Clock::time_point& timestamp, const std::string& frame_id) -{ - return build_and_optimize_fg(timestamp, frame_id); -} - -std::set NavStateFG::known_frame_ids() -{ - std::set ret; - for (const auto& [name, id] : state_.known_frames.getDirectMap()) - ret.insert(name); - - return ret; -} - -std::optional NavStateFG::build_and_optimize_fg( - const mrpt::Clock::time_point queryTimestamp, const std::string& frame_id) -{ - using namespace std::string_literals; - - delete_too_old_entries(); - - // Return an empty answer if we don't have data, or we would need to - // extrapolate too much: - if (state_.data.empty() || state_.known_frames.empty()) return {}; - { - const double tq_2_tfirst = mrpt::system::timeDifference( - queryTimestamp, state_.data.begin()->first); - const double tlast_2_tq = mrpt::system::timeDifference( - state_.data.rbegin()->first, queryTimestamp); - if (tq_2_tfirst > params_.max_time_to_use_velocity_model || - tlast_2_tq > params_.max_time_to_use_velocity_model) - { - MRPT_LOG_DEBUG_STREAM( - "[build_and_optimize_fg] Skipping due to need to extrapolate " - "too much: tq_2_tfirst=" - << tq_2_tfirst << " tlast_2_tq=" << tlast_2_tq - << " max_time_to_use_velocity_model=" - << params_.max_time_to_use_velocity_model); - - return {}; - } - } - - // shortcuts: - auto& fg = state_.impl->fg; - auto& values = state_.impl->values; - - values.clear(); - fg.resize(0); - - // Build the sequence of time points: - // FG variable indices will use the indices in this vector: - auto& dQuery = state_.data[queryTimestamp]; - dQuery.query = QueryPointData(); - - using map_it_t = std::map::value_type; - - std::vector entries; - std::optional query_KF_id; - for (const auto& it : state_.data) - { - if (it.first == queryTimestamp) query_KF_id = entries.size(); - - entries.push_back(&it); - } - ASSERT_(query_KF_id.has_value()); - - // add const vel kinematic factors between consecutive KFs: - ASSERT_(entries.size() >= 1); - - for (size_t i = 1; i < entries.size(); i++) - { - mola::FactorConstVelKinematics f; - f.from_kf_ = i - 1; - f.to_kf_ = i; - f.deltaTime_ = mrpt::system::timeDifference( - entries[i - 1]->first, entries[i]->first); - - addFactor(f); - } - - // Init values: - for (size_t i = 0; i < entries.size(); i++) - { - state_.impl->values.insert(P(i), gtsam::Z_3x1); - state_.impl->values.insert(R(i), gtsam::Rot3::Identity()); - state_.impl->values.insert(V(i), gtsam::Z_3x1); - state_.impl->values.insert(W(i), gtsam::Z_3x1); - } - for (const auto& [frameName, frameId] : state_.known_frames.getDirectMap()) - { - // F(0): this variable is not used. - // We only need to estimate F(i), the SE(3) pose of the frame_id "i" wrt - // "0" (see paper diagrams!) - if (frameId == 0) continue; - - state_.impl->values.insert( - F(frameId), gtsam::Pose3::Identity()); - } - - // Unary prior for initial twist: - const auto& tw = params_.initial_twist; - fg.addPrior( - V(0), gtsam::Vector3(tw.vx, tw.vy, tw.vz), - gtsam::noiseModel::Isotropic::Sigma( - 3, params_.initial_twist_sigma_lin)); - - fg.addPrior( - W(0), gtsam::Vector3(tw.wx, tw.wy, tw.wz), - gtsam::noiseModel::Isotropic::Sigma( - 3, params_.initial_twist_sigma_ang)); - - // Process pose observations: - // ------------------------------------------ - for (size_t kfId = 0; kfId < entries.size(); kfId++) - { - // const auto tim = entries.at(kfId)->first; - const auto& d = entries.at(kfId)->second; - - // --------------------------------- - // Data point of type: Pose - // --------------------------------- - if (d.pose.has_value()) - { - if (d.pose->frameId == 0) - { - // Pose observations in the first frame are just priors: - // (see paper!) - - gtsam::Pose3 p; - gtsam::Matrix6 pCov; - mrpt::gtsam_wrappers::to_gtsam_se3_cov6(d.pose->pose, p, pCov); - - { - auto noisePos = gtsam::noiseModel::Gaussian::Covariance( - pCov.block<3, 3>(3, 3)); - - gtsam::noiseModel::Base::shared_ptr robNoisePos; - if (params_.robust_param > 0) - robNoisePos = gtsam::noiseModel::Robust::Create( - gtsam::noiseModel::mEstimator::GemanMcClure::Create( - params_.robust_param), - noisePos); - else - robNoisePos = noisePos; - - fg.addPrior(P(kfId), p.translation(), robNoisePos); - } - - { - auto noiseRot = gtsam::noiseModel::Gaussian::Covariance( - pCov.block<3, 3>(0, 0)); - - gtsam::noiseModel::Base::shared_ptr robNoiseRot; - if (params_.robust_param > 0) - robNoiseRot = gtsam::noiseModel::Robust::Create( - gtsam::noiseModel::mEstimator::GemanMcClure::Create( - params_.robust_param), - noiseRot); - else - robNoiseRot = noiseRot; - - fg.addPrior(R(kfId), p.rotation(), robNoiseRot); - } - } - else - { - // Pose observations in subsequent frames are more complex: - // (see paper!) - THROW_EXCEPTION("todo"); - } - } - - // --------------------------------- - // Data point of type: Twist - // --------------------------------- - if (d.twist.has_value()) - { - const auto& pd = d.twist.value(); - const gtsam::Vector3 v = {pd.twist.vx, pd.twist.vy, pd.twist.vz}; - const gtsam::Vector3 w = {pd.twist.wx, pd.twist.wy, pd.twist.wz}; - gtsam::Matrix3 vCov = pd.twistCov.asEigen().block<3, 3>(0, 0); - gtsam::Matrix3 wCov = pd.twistCov.asEigen().block<3, 3>(3, 3); - - { - auto noiseV = gtsam::noiseModel::Gaussian::Covariance(vCov); - gtsam::noiseModel::Base::shared_ptr robNoiseV; - if (params_.robust_param > 0) - robNoiseV = gtsam::noiseModel::Robust::Create( - gtsam::noiseModel::mEstimator::GemanMcClure::Create( - params_.robust_param), - noiseV); - else - robNoiseV = noiseV; - - fg.addPrior(V(kfId), v, robNoiseV); - } - { - auto noiseW = gtsam::noiseModel::Gaussian::Covariance(wCov); - gtsam::noiseModel::Base::shared_ptr robNoiseW; - if (params_.robust_param > 0) - robNoiseW = gtsam::noiseModel::Robust::Create( - gtsam::noiseModel::mEstimator::GemanMcClure::Create( - params_.robust_param), - noiseW); - else - robNoiseW = noiseW; - - fg.addPrior(W(kfId), w, robNoiseW); - } - } - - } // end for each kfId - - // FG is built: optimize it - // ------------------------------------- - if (NAVSTATE_PRINT_FG) - { - fg.print(); - state_.impl->values.print(); - -#if 0 - fg.saveGraph("fg.dot"); -#endif - } - - gtsam::LevenbergMarquardtOptimizer lm(fg, state_.impl->values); - - const auto& optimal = lm.optimize(); - - const double final_rmse = std::sqrt(fg.error(optimal) / fg.size()); - - MRPT_LOG_DEBUG_STREAM( - "[build_and_optimize_fg] LM ran for " - << lm.iterations() << " iterations, " << fg.size() << " factors, " - << state_.data.size() << " KFs, RMSE: " - << std::sqrt(fg.error(state_.impl->values) / fg.size()) << " => " - << final_rmse); - - if (NAVSTATE_PRINT_FG) - { - optimal.print("Optimized:"); - std::cout << "\n query_KF_id: " << *query_KF_id << std::endl; - - MRPT_LOG_INFO_STREAM("state_.data: "); - for (const auto& [k, v] : state_.data) - { - MRPT_LOG_INFO_FMT( - "t=%f: %s", mrpt::Clock::toDouble(k), v.asString().c_str()); - } - } - - if (NAVSTATE_PRINT_FG_ERRORS) - { - fg.printErrors(optimal, "Errors for optimized values:"); - } - - // final sanity check: - if (final_rmse > params_.max_rmse) - { - MRPT_LOG_WARN_STREAM( - "[build_and_optimize_fg] Discarding solution due to high " - "RMSE=" - << final_rmse); - - return {}; - } - - // Extract results from the factor graph: - // ---------------------------------------------- - NavState out; - - // SE(3) pose: - auto poseResult = gtsam::Pose3( - optimal.at(R(*query_KF_id)), - optimal.at(P(*query_KF_id))); - const auto outPose = mrpt::gtsam_wrappers::toTPose3D(poseResult); - out.pose.mean = mrpt::poses::CPose3D(outPose); - - gtsam::Marginals marginals(fg, optimal); - - // Pose SE(3) cov: (in mrpt order is xyz, then yaw/pitch/roll): - gtsam::Matrix6 cov_inv = gtsam::Matrix6::Zero(); - cov_inv.block<3, 3>(0, 0) = marginals.marginalInformation(P(*query_KF_id)); - cov_inv.block<3, 3>(3, 3) = marginals.marginalInformation(R(*query_KF_id)); - out.pose.cov_inv = cov_inv; - - // Twist (already in body frame in the factor graph): - const auto linVel = optimal.at(V(*query_KF_id)); - const auto angVel = optimal.at(W(*query_KF_id)); - - out.twist.vx = linVel.x(); - out.twist.vy = linVel.y(); - out.twist.vz = linVel.z(); - out.twist.wx = angVel.x(); - out.twist.wy = angVel.y(); - out.twist.wz = angVel.z(); - - // Twist cov: - gtsam::Matrix6 tw_cov_inv = gtsam::Matrix6::Zero(); - tw_cov_inv.block<3, 3>(0, 0) = - marginals.marginalInformation(V(*query_KF_id)); - tw_cov_inv.block<3, 3>(3, 3) = - marginals.marginalInformation(W(*query_KF_id)); - out.twist_inv_cov = tw_cov_inv; - - // delete temporary entry: - dQuery.query.reset(); - if (dQuery.empty()) state_.data.erase(queryTimestamp); - - // Honor requested frame_id: - // ---------------------------------- - ASSERTMSG_( - state_.known_frames.hasKey(frame_id), - "Requested results in unknown frame_id: '"s + frame_id + "'"s); - - // if this is the first frame_id, we are already done, otherwise, - // recover and apply the transformation: - if (const frameid_t frameId = state_.known_frames.direct(frame_id); - frameId != 0) - { - // Apply F(frameId) transformation on the left: - const auto T = optimal.at(F(frameId)); - - THROW_EXCEPTION("TODO"); - } - - return out; -} - -/// Implementation of Eqs (1),(4) in the MOLA RSS2019 paper. -void NavStateFG::addFactor(const mola::FactorConstVelKinematics& f) -{ -#if 0 - MRPT_LOG_DEBUG_STREAM( - "[addFactor] FactorConstVelKinematics: " - << f.from_kf_ << " ==> " << f.to_kf_ << " dt=" << f.deltaTime_); -#endif - - // Add const-vel factor to gtsam itself: - double dt = f.deltaTime_; - - // trick to easily handle queries on exactly an existing keyframe: - if (dt == 0) dt = 1e-5; - - ASSERT_GT_(dt, 0.); - - // errors in constant vel: - const double std_linvel = params_.sigma_random_walk_acceleration_linear; - const double std_angvel = params_.sigma_random_walk_acceleration_angular; - - if (dt > params_.time_between_frames_to_warning) - { - MRPT_LOG_WARN_FMT( - "A constant-velocity kinematics factor has been added for a " - "dT=%.03f s.", - dt); - } - - // 1) Add GTSAM factors for constant velocity model - // ------------------------------------------------- - - auto Pi = gtsam::Point3_(P(f.from_kf_)); - auto Pj = gtsam::Point3_(P(f.to_kf_)); - auto Ri = gtsam::Rot3_(R(f.from_kf_)); - auto Rj = gtsam::Rot3_(R(f.to_kf_)); - auto bVi = gtsam::Point3_(V(f.from_kf_)); - auto bVj = gtsam::Point3_(V(f.to_kf_)); - auto bWi = gtsam::Point3_(W(f.from_kf_)); - auto bWj = gtsam::Point3_(W(f.to_kf_)); - - const auto kPi = P(f.from_kf_); - const auto kPj = P(f.to_kf_); - const auto kbVi = V(f.from_kf_); - const auto kbVj = V(f.to_kf_); - const auto kRi = R(f.from_kf_); - const auto kRj = R(f.to_kf_); - const auto kbWi = W(f.from_kf_); - const auto kbWj = W(f.to_kf_); - - // See line 3 of eq (4) in the MOLA RSS2019 paper - // Modify to use velocity in local frame: reuse FactorConstAngularVelocity - // here too: - state_.impl->fg.emplace_shared( - kRi, kbVi, kRj, kbVj, - gtsam::noiseModel::Isotropic::Sigma(3, std_linvel * dt)); - - // \omega is in the body frame, we need a special factor to rotate it: - // See line 4 of eq (4) in the MOLA RSS2019 paper. - state_.impl->fg.emplace_shared( - kRi, kbWi, kRj, kbWj, - gtsam::noiseModel::Isotropic::Sigma(3, std_angvel * dt)); - - // 2) Add kinematics / numerical integration factor - // --------------------------------------------------- - auto noise_kinematicsPosition = gtsam::noiseModel::Isotropic::Sigma( - 3, params_.sigma_integrator_position); - - auto noise_kinematicsOrientation = gtsam::noiseModel::Isotropic::Sigma( - 3, params_.sigma_integrator_orientation); - - // Impl. line 2 of eq (1) in the MOLA RSS2019 paper - state_.impl->fg.emplace_shared( - kPi, kbVi, kRi, kPj, kbVj, kRj, dt, noise_kinematicsPosition); - - // Impl. line 1 of eq (4) in the MOLA RSS2019 paper. - state_.impl->fg.emplace_shared( - kRi, kbWi, kRj, dt, noise_kinematicsOrientation); -} - -void NavStateFG::delete_too_old_entries() -{ - if (state_.data.empty()) return; - - const double newestTime = - mrpt::Clock::toDouble(state_.data.rbegin()->first); - const double minTime = newestTime - params_.sliding_window_length; - - for (auto it = state_.data.begin(); it != state_.data.end();) - { - const double t = mrpt::Clock::toDouble(it->first); - if (t < minTime) - { - // remove it: - it = state_.data.erase(it); - } - else { ++it; } - } -} - -std::string NavStateFG::PointData::asString() const -{ - std::ostringstream ss; - - if (pose) ss << "pose: " << pose->pose.mean << " "; - if (odom) ss << "odom: " << odom->pose << " "; - if (twist) ss << "twist: " << twist->twist.asString() << " "; - if (query) ss << "query"; - - return ss.str(); -} diff --git a/mola_navstate_fg/src/NavStateFGParams.cpp b/mola_navstate_fg/src/NavStateFGParams.cpp deleted file mode 100644 index 729957f1..00000000 --- a/mola_navstate_fg/src/NavStateFGParams.cpp +++ /dev/null @@ -1,58 +0,0 @@ -/* ------------------------------------------------------------------------- - * A Modular Optimization framework for Localization and mApping (MOLA) - * - * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria - * Licensed under the GNU GPL v3 for non-commercial applications. - * - * This file is part of MOLA. - * MOLA is free software: you can redistribute it and/or modify it under the - * terms of the GNU General Public License as published by the Free Software - * Foundation, either version 3 of the License, or (at your option) any later - * version. - * - * MOLA is distributed in the hope that it will be useful, but WITHOUT ANY - * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR - * A PARTICULAR PURPOSE. See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along with - * MOLA. If not, see . - * ------------------------------------------------------------------------- */ -/** - * @file NavStateFGParams.cpp - * @brief Parameters for NavStateFuse - * @author Jose Luis Blanco Claraco - * @date Jan 22, 2024 - */ - -#include - -using namespace mola; - -void NavStateFGParams::loadFrom(const mrpt::containers::yaml& cfg) -{ - MCP_LOAD_REQ(cfg, max_time_to_use_velocity_model); - - MCP_LOAD_REQ(cfg, sliding_window_length); - - MCP_LOAD_OPT(cfg, sigma_random_walk_acceleration_linear); - MCP_LOAD_OPT(cfg, sigma_random_walk_acceleration_angular); - - MCP_LOAD_OPT(cfg, time_between_frames_to_warning); - - MCP_LOAD_OPT(cfg, initial_twist_sigma_lin); - MCP_LOAD_OPT(cfg, initial_twist_sigma_ang); - - MCP_LOAD_OPT(cfg, max_rmse); - MCP_LOAD_OPT(cfg, robust_param); - - if (cfg.has("initial_twist")) - { - ASSERT_( - cfg["initial_twist"].isSequence() && - cfg["initial_twist"].asSequence().size() == 6); - - auto& tw = initial_twist; - const auto seq = cfg["initial_twist"].asSequenceRange(); - for (size_t i = 0; i < 6; i++) tw[i] = seq.at(i).as(); - } -} diff --git a/mola_navstate_fg/src/register.cpp b/mola_navstate_fg/src/register.cpp deleted file mode 100644 index cf1ab8c6..00000000 --- a/mola_navstate_fg/src/register.cpp +++ /dev/null @@ -1,35 +0,0 @@ -/* ------------------------------------------------------------------------- - * A Modular Optimization framework for Localization and mApping (MOLA) - * - * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria - * Licensed under the GNU GPL v3 for non-commercial applications. - * - * This file is part of MOLA. - * MOLA is free software: you can redistribute it and/or modify it under the - * terms of the GNU General Public License as published by the Free Software - * Foundation, either version 3 of the License, or (at your option) any later - * version. - * - * MOLA is distributed in the hope that it will be useful, but WITHOUT ANY - * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR - * A PARTICULAR PURPOSE. See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along with - * MOLA. If not, see . - * ------------------------------------------------------------------------- */ - -/** - * @file register.cpp - * @brief Register RTTI classes - * @author Jose Luis Blanco Claraco - * @date Sep 18, 2021 - */ - -#include - -// using namespace mola; - -MRPT_INITIALIZER(do_register_navstate_fuse) -{ - // MOLA_REGISTER_MODULE(xx); -} diff --git a/mola_navstate_fg/tests/CMakeLists.txt b/mola_navstate_fg/tests/CMakeLists.txt deleted file mode 100644 index 22d37b9d..00000000 --- a/mola_navstate_fg/tests/CMakeLists.txt +++ /dev/null @@ -1,7 +0,0 @@ -# Unit tests: -mola_add_test( - TARGET test-navstate-basic - SOURCES test-navstate-basic.cpp - LINK_LIBRARIES - mola::mola_navstate_fg -) diff --git a/mola_navstate_fg/tests/mola-navstate-fg-params.yaml b/mola_navstate_fg/tests/mola-navstate-fg-params.yaml deleted file mode 100644 index 3b849a8e..00000000 --- a/mola_navstate_fg/tests/mola-navstate-fg-params.yaml +++ /dev/null @@ -1,10 +0,0 @@ -# Config for NavStateFGParams -sliding_window_length: 5.0 # [s] -max_time_to_use_velocity_model: 2.0 # [s] -time_between_frames_to_warning: 2.0 # [s] -sigma_random_walk_acceleration_linear: 1.0 # [m/s²] -sigma_random_walk_acceleration_angular: 1.0 # [rad/s²] -sigma_integrator_position: 0.10 # [m] -sigma_integrator_orientation: 0.10 # [rad] -robust_param: 0 -max_rmse: 2 diff --git a/mola_navstate_fg/tests/test-navstate-basic.cpp b/mola_navstate_fg/tests/test-navstate-basic.cpp deleted file mode 100644 index 395b6633..00000000 --- a/mola_navstate_fg/tests/test-navstate-basic.cpp +++ /dev/null @@ -1,370 +0,0 @@ -/* ------------------------------------------------------------------------- - * A Modular Optimization framework for Localization and mApping (MOLA) - * - * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria - * Licensed under the GNU GPL v3 for non-commercial applications. - * - * This file is part of MOLA. - * MOLA is free software: you can redistribute it and/or modify it under the - * terms of the GNU General Public License as published by the Free Software - * Foundation, either version 3 of the License, or (at your option) any later - * version. - * - * MOLA is distributed in the hope that it will be useful, but WITHOUT ANY - * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR - * A PARTICULAR PURPOSE. See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along with - * MOLA. If not, see . - * ------------------------------------------------------------------------- */ - -/** - * @file test-navstate-basic.cpp - * @brief Unit tests for NavStateFG - * @author Jose Luis Blanco Claraco - * @date Jun 13, 2024 - */ - -#include -#include -#include -#include - -#include // required by "matrix * scalar" -#include -#include -#include - -using namespace std::string_literals; - -namespace -{ -const char* navStateParams = - R"###(# Config for NavStateFGParams -sliding_window_length: 5.0 # [s] -max_time_to_use_velocity_model: 2.0 # [s] -time_between_frames_to_warning: 2.0 # [s] -sigma_random_walk_acceleration_linear: 1.0 # [m/s²] -sigma_random_walk_acceleration_angular: 1.0 # [rad/s²] -sigma_integrator_position: 0.10 # [m] -sigma_integrator_orientation: 0.10 # [rad] -robust_param: 0 -max_rmse: 2 -)###"; - -using namespace mrpt::literals; // _deg -using mrpt::math::CMatrixDouble66; - -class Data -{ - private: - Data() = default; - - public: - static Data& Instance() - { - static Data o; - return o; - } - - const CMatrixDouble66 I6_2cm = - CMatrixDouble66(CMatrixDouble66::Identity() * 0.02); - - const mrpt::poses::CPose3D p0 = mrpt::poses::CPose3D::FromXYZYawPitchRoll( - 0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg); - const mrpt::poses::CPose3DPDFGaussian pdf0{p0, I6_2cm}; - - const mrpt::poses::CPose3D p1 = mrpt::poses::CPose3D::FromXYZYawPitchRoll( - 0.5, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg); - const mrpt::poses::CPose3DPDFGaussian pdf1{p1, I6_2cm}; - - const mrpt::poses::CPose3D p2 = mrpt::poses::CPose3D::FromXYZYawPitchRoll( - 1.0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg); - const mrpt::poses::CPose3DPDFGaussian pdf2{p2, I6_2cm}; - - // circle: - const mrpt::poses::CPose3D pc0 = mrpt::poses::CPose3D::FromXYZYawPitchRoll( - 0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg); - const mrpt::poses::CPose3DPDFGaussian pdf_c0{pc0, I6_2cm}; - - const double wc = 0.2, vc = 10.0, Rc = vc / wc; - - const mrpt::poses::CPose3D pc1 = mrpt::poses::CPose3D::FromXYZYawPitchRoll( - Rc * sin(wc * 0.1), Rc*(1 - cos(wc * 0.1)), 0, wc * 0.1, 0.0_deg, - 0.0_deg); - const mrpt::poses::CPose3DPDFGaussian pdf_c1{pc1, I6_2cm}; - - const mrpt::poses::CPose3D pc2 = mrpt::poses::CPose3D::FromXYZYawPitchRoll( - Rc * sin(wc * 0.2), Rc*(1 - cos(wc * 0.2)), 0, wc * 0.2, 0.0_deg, - 0.0_deg); - const mrpt::poses::CPose3DPDFGaussian pdf_c2{pc2, I6_2cm}; - - const mrpt::poses::CPose3D pc3 = mrpt::poses::CPose3D::FromXYZYawPitchRoll( - Rc * sin(wc * 0.3), Rc*(1 - cos(wc * 0.3)), 0, wc * 0.3, 0.0_deg, - 0.0_deg); -}; - -// -------------------------------------- -void test_init_state() -{ - mola::NavStateFG nav; - nav.initialize(mrpt::containers::yaml::FromText(navStateParams)); - - ASSERT_(nav.known_frame_ids().empty()); - - const auto ret = nav.estimated_navstate(mrpt::Clock::now(), "odom"); - ASSERT_(!ret.has_value()); -} - -// -------------------------------------- -void test_one_pose() -{ - const auto& _ = Data::Instance(); - - mola::NavStateFG nav; - nav.initialize(mrpt::containers::yaml::FromText(navStateParams)); - - const auto t0 = mrpt::Clock::fromDouble(.0); - - nav.fuse_pose(t0, _.pdf0, "odom"); - - const auto ret = nav.estimated_navstate(t0, "odom"); - ASSERT_(ret.has_value()); - - // std::cout << "Result:\n" << ret->asString() << std::endl; - - ASSERT_NEAR_( - mrpt::poses::Lie::SE<3>::log(ret->pose.mean - _.pdf0.mean).norm(), 0.0, - 1e-4); -} - -// -------------------------------------- -void test_one_pose_extrapolate() -{ - const auto& _ = Data::Instance(); - - mola::NavStateFG nav; - nav.initialize(mrpt::containers::yaml::FromText(navStateParams)); - - const auto t0 = mrpt::Clock::fromDouble(.0); - const auto t1 = mrpt::Clock::fromDouble(.5); - - nav.fuse_pose(t0, _.pdf0, "odom"); - - const auto ret = nav.estimated_navstate(t1, "odom"); - ASSERT_(ret.has_value()); - - // std::cout << "Result:\n" << ret->asString() << std::endl; - - ASSERT_NEAR_( - mrpt::poses::Lie::SE<3>::log(ret->pose.mean - _.pdf0.mean).norm(), 0.0, - 1e-4); - - ASSERT_GT_( - std::sqrt(1.0 / ret->twist_inv_cov(0, 0)), - nav.params_.initial_twist_sigma_lin); -} - -// -------------------------------------- -void test_2_poses() -{ - const auto& _ = Data::Instance(); - - mola::NavStateFG nav; - nav.initialize(mrpt::containers::yaml::FromText(navStateParams)); - - const auto t0 = mrpt::Clock::fromDouble(0.0); - const auto t1 = mrpt::Clock::fromDouble(0.5); - - const auto t2 = mrpt::Clock::fromDouble(0.6); - const auto t3 = mrpt::Clock::fromDouble(0.25); - - nav.fuse_pose(t0, _.pdf0, "odom"); - nav.fuse_pose(t1, _.pdf1, "odom"); - - const auto ret2 = nav.estimated_navstate(t2, "odom"); - ASSERT_(ret2.has_value()); - - const auto ret3 = nav.estimated_navstate(t3, "odom"); - ASSERT_(ret3.has_value()); - - // std::cout << "Result:\n" << ret2->asString() << std::endl; - - const auto expected2 = mrpt::poses::CPose3D::FromXYZYawPitchRoll( - 0.6, 0.0, 0.0, .0_deg, .0_deg, .0_deg); - ASSERT_NEAR_( - mrpt::poses::Lie::SE<3>::log(ret2->pose.mean - expected2).norm(), 0.0, - 1e-2); - - const auto expected3 = mrpt::poses::CPose3D::FromXYZYawPitchRoll( - 0.25, 0.0, 0.0, .0_deg, .0_deg, .0_deg); - ASSERT_NEAR_( - mrpt::poses::Lie::SE<3>::log(ret3->pose.mean - expected3).norm(), 0.0, - 1e-2); -} - -// -------------------------------------- -void test_2_poses_too_late() -{ - const auto& _ = Data::Instance(); - - mola::NavStateFG nav; - nav.initialize(mrpt::containers::yaml::FromText(navStateParams)); - - const auto t0 = mrpt::Clock::fromDouble(0.0); - const auto t1 = mrpt::Clock::fromDouble(0.5); - - // too late/early to extrapolate!! must return nullopt: - const auto t2 = mrpt::Clock::fromDouble( - nav.params_.max_time_to_use_velocity_model + 0.5 + 0.1); - const auto t3 = mrpt::Clock::fromDouble( - 0.0 - 0.1 - nav.params_.max_time_to_use_velocity_model); - - nav.fuse_pose(t0, _.pdf0, "odom"); - nav.fuse_pose(t1, _.pdf1, "odom"); - - const auto ret1 = nav.estimated_navstate(t2, "odom"); - ASSERT_(!ret1.has_value()); - - const auto ret2 = nav.estimated_navstate(t3, "odom"); - ASSERT_(!ret2.has_value()); -} - -// -------------------------------------- -void test_3_poses() -{ - const auto& _ = Data::Instance(); - - mola::NavStateFG nav; - nav.initialize(mrpt::containers::yaml::FromText(navStateParams)); - - const auto t0 = mrpt::Clock::fromDouble(0.0); - const auto t1 = mrpt::Clock::fromDouble(0.1); - const auto t2 = mrpt::Clock::fromDouble(0.2); - const auto t3 = mrpt::Clock::fromDouble(0.3); - - nav.fuse_pose(t0, _.pdf_c0, "odom"); - nav.fuse_pose(t1, _.pdf_c1, "odom"); - nav.fuse_pose(t2, _.pdf_c2, "odom"); - - const auto ret2 = nav.estimated_navstate(t3, "odom"); - ASSERT_(ret2.has_value()); - - // std::cout << "Result:\n" << ret2->asString() << std::endl; - - ASSERT_NEAR_( - mrpt::poses::Lie::SE<3>::log(ret2->pose.mean - _.pc3).norm(), 0.0, - 1e-1); - - ASSERT_NEAR_(ret2->twist.vx, _.vc, 0.1); - // ASSERT_NEAR_(ret2->twist.vy, .0, 0.2); - ASSERT_NEAR_(ret2->twist.vz, .0, 0.2); - - ASSERT_NEAR_(ret2->twist.wx, .0, 0.01); - ASSERT_NEAR_(ret2->twist.wy, .0, 0.01); - ASSERT_NEAR_(ret2->twist.wz, _.wc, 0.05); -} - -// -------------------------------------- -void test_noisy_straight() -{ - const auto& _ = Data::Instance(); - - mola::NavStateFG nav; - nav.initialize(mrpt::containers::yaml::FromText(navStateParams)); - - auto& rng = mrpt::random::getRandomGenerator(); - - const size_t nSteps = 10; - const double vx = 8.0; // m/s - const double T = 0.100; // s - - const double stdXYZ = 0.01; - const double stdYPR = mrpt::DEG2RAD(0.1); - - for (size_t i = 0; i < nSteps; i++) - { - const double tt = T * i; - const auto t = mrpt::Clock::fromDouble(tt); - - const mrpt::poses::CPose3D p = - mrpt::poses::CPose3D::FromXYZYawPitchRoll( - vx * tt + rng.drawGaussian1D(0, stdXYZ), - rng.drawGaussian1D(0, stdXYZ), +rng.drawGaussian1D(0, stdXYZ), - rng.drawGaussian1D(0, stdYPR), rng.drawGaussian1D(0, stdYPR), - rng.drawGaussian1D(0, stdYPR)); - const mrpt::poses::CPose3DPDFGaussian pdf{p, _.I6_2cm}; - - nav.fuse_pose(t, pdf, "odom"); - } - - // query: - const double tt_q = T * nSteps; - const auto t_q = mrpt::Clock::fromDouble(tt_q); - - const auto ret = nav.estimated_navstate(t_q, "odom"); - ASSERT_(ret.has_value()); - - // std::cout << "Result:\n" << ret->asString() << std::endl; - - ASSERT_NEAR_(ret->twist.vx, vx, 0.1); - ASSERT_NEAR_(ret->twist.vy, .0, 0.1); - ASSERT_NEAR_(ret->twist.vz, .0, 0.1); - - ASSERT_NEAR_(ret->twist.wx, .0, 0.01); - ASSERT_NEAR_(ret->twist.wy, .0, 0.01); - ASSERT_NEAR_(ret->twist.wz, .0, 0.01); -} - -} // namespace - -int main(int argc, char** argv) -{ - const std::map> tests = { - {"test_init_state", test_init_state}, - {"test_one_pose", test_one_pose}, - {"test_one_pose_extrap", test_one_pose_extrapolate}, - {"test_2_poses", test_2_poses}, - {"test_2_poses_too_late", test_2_poses_too_late}, - {"test_3_poses", test_3_poses}, - {"test_noisy_straight", test_noisy_straight}, - }; - - int runOnlyIdx = -1; - if (argc == 2) runOnlyIdx = std::stoi(argv[1]); - - bool anyFail = false; - - int index = 0; - for (const auto& [name, f] : tests) - { - index++; - - if (runOnlyIdx >= 0 && index != runOnlyIdx) continue; - - const auto sPrefix = mrpt::format( - "[ (%3i / %3zu) %20s ]", index, tests.size(), name.c_str()); - try - { - std::cout << sPrefix << " Running..." << std::endl; - f(); - - mrpt::system::consoleColorAndStyle( - mrpt::system::ConsoleForegroundColor::GREEN); - std::cout << sPrefix << " OK." << std::endl; - } - catch (std::exception& e) - { - mrpt::system::consoleColorAndStyle( - mrpt::system::ConsoleForegroundColor::RED); - std::cout << sPrefix << " ERROR: " << std::endl - << e.what() << std::endl; - anyFail = true; - } - - mrpt::system::consoleColorAndStyle( - mrpt::system::ConsoleForegroundColor::DEFAULT); - } - - return anyFail; -} diff --git a/mola_navstate_fuse/.clang-format b/mola_navstate_fuse/.clang-format deleted file mode 100644 index d1df9d28..00000000 --- a/mola_navstate_fuse/.clang-format +++ /dev/null @@ -1,83 +0,0 @@ -Language: Cpp -BasedOnStyle: Google -# --- -#AccessModifierOffset: -4 -AlignAfterOpenBracket: AlwaysBreak # Values: Align, DontAlign, AlwaysBreak -AlignConsecutiveAssignments: true -AlignConsecutiveDeclarations: true -#AlignEscapedNewlinesLeft: true -#AlignOperands: false -AlignTrailingComments: false # Should be off, causes many dummy problems!! -#AllowAllParametersOfDeclarationOnNextLine: true -AllowShortBlocksOnASingleLine: true -#AllowShortCaseLabelsOnASingleLine: false -#AllowShortFunctionsOnASingleLine: Empty -#AllowShortIfStatementsOnASingleLine: false -#AllowShortLoopsOnASingleLine: false -#AlwaysBreakAfterDefinitionReturnType: None -#AlwaysBreakAfterReturnType: None -#AlwaysBreakBeforeMultilineStrings: true -#AlwaysBreakTemplateDeclarations: true -#BinPackArguments: false -#BinPackParameters: false -#BraceWrapping: - #AfterClass: false - #AfterControlStatement: false - #AfterEnum: false - #AfterFunction: false - #AfterNamespace: false - #AfterObjCDeclaration: false - #AfterStruct: false - #AfterUnion: false - #BeforeCatch: false - #BeforeElse: true - #IndentBraces: false -#BreakBeforeBinaryOperators: None -BreakBeforeBraces: Allman -#BreakBeforeTernaryOperators: true -#BreakConstructorInitializersBeforeComma: false -ColumnLimit: 80 -#CommentPragmas: '' -#ConstructorInitializerAllOnOneLineOrOnePerLine: true -#ConstructorInitializerIndentWidth: 4 -#ContinuationIndentWidth: 4 -#Cpp11BracedListStyle: true -#DerivePointerAlignment: false -#DisableFormat: false -#ExperimentalAutoDetectBinPacking: false -##FixNamespaceComments: true # Not applicable in 3.8 -#ForEachMacros: [ foreach, Q_FOREACH, BOOST_FOREACH ] -#IncludeCategories: - #- Regex: '.*' - #Priority: 1 -IndentCaseLabels: true -IndentWidth: 4 -IndentWrappedFunctionNames: true -#KeepEmptyLinesAtTheStartOfBlocks: true -#MacroBlockBegin: '' -#MacroBlockEnd: '' -MaxEmptyLinesToKeep: 1 -NamespaceIndentation: None -#PenaltyBreakBeforeFirstCallParameter: 19 -#PenaltyBreakComment: 300 -#PenaltyBreakFirstLessLess: 120 -#PenaltyBreakString: 1000 -#PenaltyExcessCharacter: 1000000 -#PenaltyReturnTypeOnItsOwnLine: 200 -DerivePointerAlignment: false -#PointerAlignment: Left -ReflowComments: true # Should be true, otherwise clang-format doesn't touch comments -SortIncludes: true -#SpaceAfterCStyleCast: false -SpaceBeforeAssignmentOperators: true -#SpaceBeforeParens: ControlStatements -#SpaceInEmptyParentheses: false -#SpacesBeforeTrailingComments: 2 -#SpacesInAngles: false -#SpacesInContainerLiterals: true -#SpacesInCStyleCastParentheses: false -#SpacesInParentheses: false -#SpacesInSquareBrackets: false -Standard: Cpp11 -TabWidth: 4 -UseTab: Never # Available options are Never, Always, ForIndentation diff --git a/mola_navstate_fuse/CHANGELOG.rst b/mola_navstate_fuse/CHANGELOG.rst deleted file mode 100644 index b40ade3f..00000000 --- a/mola_navstate_fuse/CHANGELOG.rst +++ /dev/null @@ -1,82 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package mola_navstate_fuse -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -1.4.1 (2024-12-20) ------------------- - -1.4.0 (2024-12-18) ------------------- -* Allow zero variances for some pose components -* Contributors: Jose Luis Blanco-Claraco - -1.3.0 (2024-12-11) ------------------- -* Start integrating GNSS observation. Added a new CLI program mola-navstate-cli for testing state fusion -* Contributors: Jose Luis Blanco-Claraco - -1.2.1 (2024-09-29) ------------------- - -1.2.0 (2024-09-16) ------------------- - -1.1.3 (2024-08-28) ------------------- -* Depend on new mrpt_lib packages (deprecate mrpt2) -* Contributors: Jose Luis Blanco-Claraco - -1.1.2 (2024-08-26) ------------------- - -1.1.1 (2024-08-23) ------------------- - -1.1.0 (2024-08-18) ------------------- -* Merge pull request `#62 `_ from MOLAorg/docs-fixes - Docs fixes -* Fix ament_xmllint warnings in package.xml -* Contributors: Jose Luis Blanco-Claraco - -1.0.8 (2024-07-29) ------------------- -* ament_lint_cmake: clean warnings -* Contributors: Jose Luis Blanco-Claraco - -1.0.7 (2024-07-24) ------------------- - -1.0.6 (2024-06-21) ------------------- -* Create new NavStateFilter interface and separate the simple fuser and the factor-graph approach in two packages -* Twist in local frame -* Contributors: Jose Luis Blanco-Claraco - -1.0.5 (2024-05-28) ------------------- - -1.0.4 (2024-05-14) ------------------- -* bump cmake_minimum_required to 3.5 -* Contributors: Jose Luis Blanco-Claraco - -1.0.3 (2024-04-22) ------------------- -* Fix package.xml website URL -* Contributors: Jose Luis Blanco-Claraco - -1.0.2 (2024-04-04) ------------------- - -1.0.1 (2024-03-28) ------------------- - -1.0.0 (2024-03-19) ------------------- -* use odometry -* add new package mola_navstate_fuse -* Contributors: Jose Luis Blanco-Claraco - -0.2.2 (2023-09-08) ------------------- diff --git a/mola_navstate_fuse/CMakeLists.txt b/mola_navstate_fuse/CMakeLists.txt deleted file mode 100644 index d64e3dd3..00000000 --- a/mola_navstate_fuse/CMakeLists.txt +++ /dev/null @@ -1,49 +0,0 @@ -# ------------------------------------------------------------------------------ -# A Modular Optimization framework for Localization and mApping -# (MOLA) -# -# Copyright (C) 2018-2024, Jose Luis Blanco-Claraco, contributors (AUTHORS.md) -# All rights reserved. -# Released under GNU GPL v3. See LICENSE file -# ------------------------------------------------------------------------------ - -# Minimum CMake vesion: limited by CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS -cmake_minimum_required(VERSION 3.5) - -# Tell CMake we'll use C++ for use in its tests/flags -project(mola_navstate_fuse LANGUAGES CXX) - -# MOLA CMake scripts: "mola_xxx()" -find_package(mola_common REQUIRED) - -# find CMake dependencies: -find_package(mrpt-obs) - -# Find MOLA packages: -find_package(mola_imu_preintegration REQUIRED) -find_package(mola_kernel REQUIRED) - -# ----------------------- -# define lib: -file(GLOB_RECURSE LIB_SRCS src/*.cpp src/*.h) -file(GLOB_RECURSE LIB_PUBLIC_HDRS include/*.h) - -mola_add_library( - TARGET ${PROJECT_NAME} - SOURCES ${LIB_SRCS} ${LIB_PUBLIC_HDRS} - PUBLIC_LINK_LIBRARIES - mrpt::obs - mola::mola_imu_preintegration - mola::mola_kernel - #PRIVATE_LINK_LIBRARIES - # mrpt::obs - CMAKE_DEPENDENCIES - mola_common - mola_kernel - mola_imu_preintegration -) - -# ----------------------- -# define tests: -#enable_testing() -#add_subdirectory(tests) diff --git a/mola_navstate_fuse/LICENSE b/mola_navstate_fuse/LICENSE deleted file mode 100644 index f288702d..00000000 --- a/mola_navstate_fuse/LICENSE +++ /dev/null @@ -1,674 +0,0 @@ - GNU GENERAL PUBLIC LICENSE - Version 3, 29 June 2007 - - Copyright (C) 2007 Free Software Foundation, Inc. - Everyone is permitted to copy and distribute verbatim copies - of this license document, but changing it is not allowed. - - Preamble - - The GNU General Public License is a free, copyleft license for -software and other kinds of works. - - The licenses for most software and other practical works are designed -to take away your freedom to share and change the works. By contrast, -the GNU General Public License is intended to guarantee your freedom to -share and change all versions of a program--to make sure it remains free -software for all its users. We, the Free Software Foundation, use the -GNU General Public License for most of our software; it applies also to -any other work released this way by its authors. You can apply it to -your programs, too. - - When we speak of free software, we are referring to freedom, not -price. Our General Public Licenses are designed to make sure that you -have the freedom to distribute copies of free software (and charge for -them if you wish), that you receive source code or can get it if you -want it, that you can change the software or use pieces of it in new -free programs, and that you know you can do these things. - - To protect your rights, we need to prevent others from denying you -these rights or asking you to surrender the rights. Therefore, you have -certain responsibilities if you distribute copies of the software, or if -you modify it: responsibilities to respect the freedom of others. - - For example, if you distribute copies of such a program, whether -gratis or for a fee, you must pass on to the recipients the same -freedoms that you received. You must make sure that they, too, receive -or can get the source code. And you must show them these terms so they -know their rights. - - Developers that use the GNU GPL protect your rights with two steps: -(1) assert copyright on the software, and (2) offer you this License -giving you legal permission to copy, distribute and/or modify it. - - For the developers' and authors' protection, the GPL clearly explains -that there is no warranty for this free software. For both users' and -authors' sake, the GPL requires that modified versions be marked as -changed, so that their problems will not be attributed erroneously to -authors of previous versions. - - Some devices are designed to deny users access to install or run -modified versions of the software inside them, although the manufacturer -can do so. This is fundamentally incompatible with the aim of -protecting users' freedom to change the software. The systematic -pattern of such abuse occurs in the area of products for individuals to -use, which is precisely where it is most unacceptable. Therefore, we -have designed this version of the GPL to prohibit the practice for those -products. If such problems arise substantially in other domains, we -stand ready to extend this provision to those domains in future versions -of the GPL, as needed to protect the freedom of users. - - Finally, every program is threatened constantly by software patents. -States should not allow patents to restrict development and use of -software on general-purpose computers, but in those that do, we wish to -avoid the special danger that patents applied to a free program could -make it effectively proprietary. To prevent this, the GPL assures that -patents cannot be used to render the program non-free. - - The precise terms and conditions for copying, distribution and -modification follow. - - TERMS AND CONDITIONS - - 0. Definitions. - - "This License" refers to version 3 of the GNU General Public License. - - "Copyright" also means copyright-like laws that apply to other kinds of -works, such as semiconductor masks. - - "The Program" refers to any copyrightable work licensed under this -License. Each licensee is addressed as "you". "Licensees" and -"recipients" may be individuals or organizations. - - To "modify" a work means to copy from or adapt all or part of the work -in a fashion requiring copyright permission, other than the making of an -exact copy. The resulting work is called a "modified version" of the -earlier work or a work "based on" the earlier work. - - A "covered work" means either the unmodified Program or a work based -on the Program. - - To "propagate" a work means to do anything with it that, without -permission, would make you directly or secondarily liable for -infringement under applicable copyright law, except executing it on a -computer or modifying a private copy. Propagation includes copying, -distribution (with or without modification), making available to the -public, and in some countries other activities as well. - - To "convey" a work means any kind of propagation that enables other -parties to make or receive copies. Mere interaction with a user through -a computer network, with no transfer of a copy, is not conveying. - - An interactive user interface displays "Appropriate Legal Notices" -to the extent that it includes a convenient and prominently visible -feature that (1) displays an appropriate copyright notice, and (2) -tells the user that there is no warranty for the work (except to the -extent that warranties are provided), that licensees may convey the -work under this License, and how to view a copy of this License. If -the interface presents a list of user commands or options, such as a -menu, a prominent item in the list meets this criterion. - - 1. Source Code. - - The "source code" for a work means the preferred form of the work -for making modifications to it. "Object code" means any non-source -form of a work. - - A "Standard Interface" means an interface that either is an official -standard defined by a recognized standards body, or, in the case of -interfaces specified for a particular programming language, one that -is widely used among developers working in that language. - - The "System Libraries" of an executable work include anything, other -than the work as a whole, that (a) is included in the normal form of -packaging a Major Component, but which is not part of that Major -Component, and (b) serves only to enable use of the work with that -Major Component, or to implement a Standard Interface for which an -implementation is available to the public in source code form. A -"Major Component", in this context, means a major essential component -(kernel, window system, and so on) of the specific operating system -(if any) on which the executable work runs, or a compiler used to -produce the work, or an object code interpreter used to run it. - - The "Corresponding Source" for a work in object code form means all -the source code needed to generate, install, and (for an executable -work) run the object code and to modify the work, including scripts to -control those activities. However, it does not include the work's -System Libraries, or general-purpose tools or generally available free -programs which are used unmodified in performing those activities but -which are not part of the work. For example, Corresponding Source -includes interface definition files associated with source files for -the work, and the source code for shared libraries and dynamically -linked subprograms that the work is specifically designed to require, -such as by intimate data communication or control flow between those -subprograms and other parts of the work. - - The Corresponding Source need not include anything that users -can regenerate automatically from other parts of the Corresponding -Source. - - The Corresponding Source for a work in source code form is that -same work. - - 2. Basic Permissions. - - All rights granted under this License are granted for the term of -copyright on the Program, and are irrevocable provided the stated -conditions are met. This License explicitly affirms your unlimited -permission to run the unmodified Program. The output from running a -covered work is covered by this License only if the output, given its -content, constitutes a covered work. This License acknowledges your -rights of fair use or other equivalent, as provided by copyright law. - - You may make, run and propagate covered works that you do not -convey, without conditions so long as your license otherwise remains -in force. You may convey covered works to others for the sole purpose -of having them make modifications exclusively for you, or provide you -with facilities for running those works, provided that you comply with -the terms of this License in conveying all material for which you do -not control copyright. Those thus making or running the covered works -for you must do so exclusively on your behalf, under your direction -and control, on terms that prohibit them from making any copies of -your copyrighted material outside their relationship with you. - - Conveying under any other circumstances is permitted solely under -the conditions stated below. Sublicensing is not allowed; section 10 -makes it unnecessary. - - 3. Protecting Users' Legal Rights From Anti-Circumvention Law. - - No covered work shall be deemed part of an effective technological -measure under any applicable law fulfilling obligations under article -11 of the WIPO copyright treaty adopted on 20 December 1996, or -similar laws prohibiting or restricting circumvention of such -measures. - - When you convey a covered work, you waive any legal power to forbid -circumvention of technological measures to the extent such circumvention -is effected by exercising rights under this License with respect to -the covered work, and you disclaim any intention to limit operation or -modification of the work as a means of enforcing, against the work's -users, your or third parties' legal rights to forbid circumvention of -technological measures. - - 4. Conveying Verbatim Copies. - - You may convey verbatim copies of the Program's source code as you -receive it, in any medium, provided that you conspicuously and -appropriately publish on each copy an appropriate copyright notice; -keep intact all notices stating that this License and any -non-permissive terms added in accord with section 7 apply to the code; -keep intact all notices of the absence of any warranty; and give all -recipients a copy of this License along with the Program. - - You may charge any price or no price for each copy that you convey, -and you may offer support or warranty protection for a fee. - - 5. Conveying Modified Source Versions. - - You may convey a work based on the Program, or the modifications to -produce it from the Program, in the form of source code under the -terms of section 4, provided that you also meet all of these conditions: - - a) The work must carry prominent notices stating that you modified - it, and giving a relevant date. - - b) The work must carry prominent notices stating that it is - released under this License and any conditions added under section - 7. This requirement modifies the requirement in section 4 to - "keep intact all notices". - - c) You must license the entire work, as a whole, under this - License to anyone who comes into possession of a copy. This - License will therefore apply, along with any applicable section 7 - additional terms, to the whole of the work, and all its parts, - regardless of how they are packaged. This License gives no - permission to license the work in any other way, but it does not - invalidate such permission if you have separately received it. - - d) If the work has interactive user interfaces, each must display - Appropriate Legal Notices; however, if the Program has interactive - interfaces that do not display Appropriate Legal Notices, your - work need not make them do so. - - A compilation of a covered work with other separate and independent -works, which are not by their nature extensions of the covered work, -and which are not combined with it such as to form a larger program, -in or on a volume of a storage or distribution medium, is called an -"aggregate" if the compilation and its resulting copyright are not -used to limit the access or legal rights of the compilation's users -beyond what the individual works permit. Inclusion of a covered work -in an aggregate does not cause this License to apply to the other -parts of the aggregate. - - 6. Conveying Non-Source Forms. - - You may convey a covered work in object code form under the terms -of sections 4 and 5, provided that you also convey the -machine-readable Corresponding Source under the terms of this License, -in one of these ways: - - a) Convey the object code in, or embodied in, a physical product - (including a physical distribution medium), accompanied by the - Corresponding Source fixed on a durable physical medium - customarily used for software interchange. - - b) Convey the object code in, or embodied in, a physical product - (including a physical distribution medium), accompanied by a - written offer, valid for at least three years and valid for as - long as you offer spare parts or customer support for that product - model, to give anyone who possesses the object code either (1) a - copy of the Corresponding Source for all the software in the - product that is covered by this License, on a durable physical - medium customarily used for software interchange, for a price no - more than your reasonable cost of physically performing this - conveying of source, or (2) access to copy the - Corresponding Source from a network server at no charge. - - c) Convey individual copies of the object code with a copy of the - written offer to provide the Corresponding Source. This - alternative is allowed only occasionally and noncommercially, and - only if you received the object code with such an offer, in accord - with subsection 6b. - - d) Convey the object code by offering access from a designated - place (gratis or for a charge), and offer equivalent access to the - Corresponding Source in the same way through the same place at no - further charge. You need not require recipients to copy the - Corresponding Source along with the object code. If the place to - copy the object code is a network server, the Corresponding Source - may be on a different server (operated by you or a third party) - that supports equivalent copying facilities, provided you maintain - clear directions next to the object code saying where to find the - Corresponding Source. Regardless of what server hosts the - Corresponding Source, you remain obligated to ensure that it is - available for as long as needed to satisfy these requirements. - - e) Convey the object code using peer-to-peer transmission, provided - you inform other peers where the object code and Corresponding - Source of the work are being offered to the general public at no - charge under subsection 6d. - - A separable portion of the object code, whose source code is excluded -from the Corresponding Source as a System Library, need not be -included in conveying the object code work. - - A "User Product" is either (1) a "consumer product", which means any -tangible personal property which is normally used for personal, family, -or household purposes, or (2) anything designed or sold for incorporation -into a dwelling. In determining whether a product is a consumer product, -doubtful cases shall be resolved in favor of coverage. For a particular -product received by a particular user, "normally used" refers to a -typical or common use of that class of product, regardless of the status -of the particular user or of the way in which the particular user -actually uses, or expects or is expected to use, the product. A product -is a consumer product regardless of whether the product has substantial -commercial, industrial or non-consumer uses, unless such uses represent -the only significant mode of use of the product. - - "Installation Information" for a User Product means any methods, -procedures, authorization keys, or other information required to install -and execute modified versions of a covered work in that User Product from -a modified version of its Corresponding Source. The information must -suffice to ensure that the continued functioning of the modified object -code is in no case prevented or interfered with solely because -modification has been made. - - If you convey an object code work under this section in, or with, or -specifically for use in, a User Product, and the conveying occurs as -part of a transaction in which the right of possession and use of the -User Product is transferred to the recipient in perpetuity or for a -fixed term (regardless of how the transaction is characterized), the -Corresponding Source conveyed under this section must be accompanied -by the Installation Information. But this requirement does not apply -if neither you nor any third party retains the ability to install -modified object code on the User Product (for example, the work has -been installed in ROM). - - The requirement to provide Installation Information does not include a -requirement to continue to provide support service, warranty, or updates -for a work that has been modified or installed by the recipient, or for -the User Product in which it has been modified or installed. Access to a -network may be denied when the modification itself materially and -adversely affects the operation of the network or violates the rules and -protocols for communication across the network. - - Corresponding Source conveyed, and Installation Information provided, -in accord with this section must be in a format that is publicly -documented (and with an implementation available to the public in -source code form), and must require no special password or key for -unpacking, reading or copying. - - 7. Additional Terms. - - "Additional permissions" are terms that supplement the terms of this -License by making exceptions from one or more of its conditions. -Additional permissions that are applicable to the entire Program shall -be treated as though they were included in this License, to the extent -that they are valid under applicable law. If additional permissions -apply only to part of the Program, that part may be used separately -under those permissions, but the entire Program remains governed by -this License without regard to the additional permissions. - - When you convey a copy of a covered work, you may at your option -remove any additional permissions from that copy, or from any part of -it. (Additional permissions may be written to require their own -removal in certain cases when you modify the work.) You may place -additional permissions on material, added by you to a covered work, -for which you have or can give appropriate copyright permission. - - Notwithstanding any other provision of this License, for material you -add to a covered work, you may (if authorized by the copyright holders of -that material) supplement the terms of this License with terms: - - a) Disclaiming warranty or limiting liability differently from the - terms of sections 15 and 16 of this License; or - - b) Requiring preservation of specified reasonable legal notices or - author attributions in that material or in the Appropriate Legal - Notices displayed by works containing it; or - - c) Prohibiting misrepresentation of the origin of that material, or - requiring that modified versions of such material be marked in - reasonable ways as different from the original version; or - - d) Limiting the use for publicity purposes of names of licensors or - authors of the material; or - - e) Declining to grant rights under trademark law for use of some - trade names, trademarks, or service marks; or - - f) Requiring indemnification of licensors and authors of that - material by anyone who conveys the material (or modified versions of - it) with contractual assumptions of liability to the recipient, for - any liability that these contractual assumptions directly impose on - those licensors and authors. - - All other non-permissive additional terms are considered "further -restrictions" within the meaning of section 10. If the Program as you -received it, or any part of it, contains a notice stating that it is -governed by this License along with a term that is a further -restriction, you may remove that term. If a license document contains -a further restriction but permits relicensing or conveying under this -License, you may add to a covered work material governed by the terms -of that license document, provided that the further restriction does -not survive such relicensing or conveying. - - If you add terms to a covered work in accord with this section, you -must place, in the relevant source files, a statement of the -additional terms that apply to those files, or a notice indicating -where to find the applicable terms. - - Additional terms, permissive or non-permissive, may be stated in the -form of a separately written license, or stated as exceptions; -the above requirements apply either way. - - 8. Termination. - - You may not propagate or modify a covered work except as expressly -provided under this License. Any attempt otherwise to propagate or -modify it is void, and will automatically terminate your rights under -this License (including any patent licenses granted under the third -paragraph of section 11). - - However, if you cease all violation of this License, then your -license from a particular copyright holder is reinstated (a) -provisionally, unless and until the copyright holder explicitly and -finally terminates your license, and (b) permanently, if the copyright -holder fails to notify you of the violation by some reasonable means -prior to 60 days after the cessation. - - Moreover, your license from a particular copyright holder is -reinstated permanently if the copyright holder notifies you of the -violation by some reasonable means, this is the first time you have -received notice of violation of this License (for any work) from that -copyright holder, and you cure the violation prior to 30 days after -your receipt of the notice. - - Termination of your rights under this section does not terminate the -licenses of parties who have received copies or rights from you under -this License. If your rights have been terminated and not permanently -reinstated, you do not qualify to receive new licenses for the same -material under section 10. - - 9. Acceptance Not Required for Having Copies. - - You are not required to accept this License in order to receive or -run a copy of the Program. Ancillary propagation of a covered work -occurring solely as a consequence of using peer-to-peer transmission -to receive a copy likewise does not require acceptance. However, -nothing other than this License grants you permission to propagate or -modify any covered work. These actions infringe copyright if you do -not accept this License. Therefore, by modifying or propagating a -covered work, you indicate your acceptance of this License to do so. - - 10. Automatic Licensing of Downstream Recipients. - - Each time you convey a covered work, the recipient automatically -receives a license from the original licensors, to run, modify and -propagate that work, subject to this License. You are not responsible -for enforcing compliance by third parties with this License. - - An "entity transaction" is a transaction transferring control of an -organization, or substantially all assets of one, or subdividing an -organization, or merging organizations. If propagation of a covered -work results from an entity transaction, each party to that -transaction who receives a copy of the work also receives whatever -licenses to the work the party's predecessor in interest had or could -give under the previous paragraph, plus a right to possession of the -Corresponding Source of the work from the predecessor in interest, if -the predecessor has it or can get it with reasonable efforts. - - You may not impose any further restrictions on the exercise of the -rights granted or affirmed under this License. For example, you may -not impose a license fee, royalty, or other charge for exercise of -rights granted under this License, and you may not initiate litigation -(including a cross-claim or counterclaim in a lawsuit) alleging that -any patent claim is infringed by making, using, selling, offering for -sale, or importing the Program or any portion of it. - - 11. Patents. - - A "contributor" is a copyright holder who authorizes use under this -License of the Program or a work on which the Program is based. The -work thus licensed is called the contributor's "contributor version". - - A contributor's "essential patent claims" are all patent claims -owned or controlled by the contributor, whether already acquired or -hereafter acquired, that would be infringed by some manner, permitted -by this License, of making, using, or selling its contributor version, -but do not include claims that would be infringed only as a -consequence of further modification of the contributor version. For -purposes of this definition, "control" includes the right to grant -patent sublicenses in a manner consistent with the requirements of -this License. - - Each contributor grants you a non-exclusive, worldwide, royalty-free -patent license under the contributor's essential patent claims, to -make, use, sell, offer for sale, import and otherwise run, modify and -propagate the contents of its contributor version. - - In the following three paragraphs, a "patent license" is any express -agreement or commitment, however denominated, not to enforce a patent -(such as an express permission to practice a patent or covenant not to -sue for patent infringement). To "grant" such a patent license to a -party means to make such an agreement or commitment not to enforce a -patent against the party. - - If you convey a covered work, knowingly relying on a patent license, -and the Corresponding Source of the work is not available for anyone -to copy, free of charge and under the terms of this License, through a -publicly available network server or other readily accessible means, -then you must either (1) cause the Corresponding Source to be so -available, or (2) arrange to deprive yourself of the benefit of the -patent license for this particular work, or (3) arrange, in a manner -consistent with the requirements of this License, to extend the patent -license to downstream recipients. "Knowingly relying" means you have -actual knowledge that, but for the patent license, your conveying the -covered work in a country, or your recipient's use of the covered work -in a country, would infringe one or more identifiable patents in that -country that you have reason to believe are valid. - - If, pursuant to or in connection with a single transaction or -arrangement, you convey, or propagate by procuring conveyance of, a -covered work, and grant a patent license to some of the parties -receiving the covered work authorizing them to use, propagate, modify -or convey a specific copy of the covered work, then the patent license -you grant is automatically extended to all recipients of the covered -work and works based on it. - - A patent license is "discriminatory" if it does not include within -the scope of its coverage, prohibits the exercise of, or is -conditioned on the non-exercise of one or more of the rights that are -specifically granted under this License. You may not convey a covered -work if you are a party to an arrangement with a third party that is -in the business of distributing software, under which you make payment -to the third party based on the extent of your activity of conveying -the work, and under which the third party grants, to any of the -parties who would receive the covered work from you, a discriminatory -patent license (a) in connection with copies of the covered work -conveyed by you (or copies made from those copies), or (b) primarily -for and in connection with specific products or compilations that -contain the covered work, unless you entered into that arrangement, -or that patent license was granted, prior to 28 March 2007. - - Nothing in this License shall be construed as excluding or limiting -any implied license or other defenses to infringement that may -otherwise be available to you under applicable patent law. - - 12. No Surrender of Others' Freedom. - - If conditions are imposed on you (whether by court order, agreement or -otherwise) that contradict the conditions of this License, they do not -excuse you from the conditions of this License. If you cannot convey a -covered work so as to satisfy simultaneously your obligations under this -License and any other pertinent obligations, then as a consequence you may -not convey it at all. For example, if you agree to terms that obligate you -to collect a royalty for further conveying from those to whom you convey -the Program, the only way you could satisfy both those terms and this -License would be to refrain entirely from conveying the Program. - - 13. Use with the GNU Affero General Public License. - - Notwithstanding any other provision of this License, you have -permission to link or combine any covered work with a work licensed -under version 3 of the GNU Affero General Public License into a single -combined work, and to convey the resulting work. The terms of this -License will continue to apply to the part which is the covered work, -but the special requirements of the GNU Affero General Public License, -section 13, concerning interaction through a network will apply to the -combination as such. - - 14. Revised Versions of this License. - - The Free Software Foundation may publish revised and/or new versions of -the GNU General Public License from time to time. Such new versions will -be similar in spirit to the present version, but may differ in detail to -address new problems or concerns. - - Each version is given a distinguishing version number. If the -Program specifies that a certain numbered version of the GNU General -Public License "or any later version" applies to it, you have the -option of following the terms and conditions either of that numbered -version or of any later version published by the Free Software -Foundation. If the Program does not specify a version number of the -GNU General Public License, you may choose any version ever published -by the Free Software Foundation. - - If the Program specifies that a proxy can decide which future -versions of the GNU General Public License can be used, that proxy's -public statement of acceptance of a version permanently authorizes you -to choose that version for the Program. - - Later license versions may give you additional or different -permissions. However, no additional obligations are imposed on any -author or copyright holder as a result of your choosing to follow a -later version. - - 15. Disclaimer of Warranty. - - THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY -APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT -HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY -OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, -THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR -PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM -IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF -ALL NECESSARY SERVICING, REPAIR OR CORRECTION. - - 16. Limitation of Liability. - - IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING -WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS -THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY -GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE -USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF -DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD -PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), -EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF -SUCH DAMAGES. - - 17. Interpretation of Sections 15 and 16. - - If the disclaimer of warranty and limitation of liability provided -above cannot be given local legal effect according to their terms, -reviewing courts shall apply local law that most closely approximates -an absolute waiver of all civil liability in connection with the -Program, unless a warranty or assumption of liability accompanies a -copy of the Program in return for a fee. - - END OF TERMS AND CONDITIONS - - How to Apply These Terms to Your New Programs - - If you develop a new program, and you want it to be of the greatest -possible use to the public, the best way to achieve this is to make it -free software which everyone can redistribute and change under these terms. - - To do so, attach the following notices to the program. It is safest -to attach them to the start of each source file to most effectively -state the exclusion of warranty; and each file should have at least -the "copyright" line and a pointer to where the full notice is found. - - - Copyright (C) - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - -Also add information on how to contact you by electronic and paper mail. - - If the program does terminal interaction, make it output a short -notice like this when it starts in an interactive mode: - - Copyright (C) - This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. - This is free software, and you are welcome to redistribute it - under certain conditions; type `show c' for details. - -The hypothetical commands `show w' and `show c' should show the appropriate -parts of the General Public License. Of course, your program's commands -might be different; for a GUI interface, you would use an "about box". - - You should also get your employer (if you work as a programmer) or school, -if any, to sign a "copyright disclaimer" for the program, if necessary. -For more information on this, and how to apply and follow the GNU GPL, see -. - - The GNU General Public License does not permit incorporating your program -into proprietary programs. If your program is a subroutine library, you -may consider it more useful to permit linking proprietary applications with -the library. If this is what you want to do, use the GNU Lesser General -Public License instead of this License. But first, please read -. diff --git a/mola_navstate_fuse/README.md b/mola_navstate_fuse/README.md deleted file mode 100644 index ce89322f..00000000 --- a/mola_navstate_fuse/README.md +++ /dev/null @@ -1,14 +0,0 @@ -# mola_navstate_fuse -Simple kinematic state vector extrapolation algorithm to fuse odometry sources. - -This repository provides: -* `NavStateFuse`: C++ class to integrate odometry, IMU, and pose/twist estimations. - -See package [documentation](https://docs.mola-slam.org/latest/modules.html). - - -## Build and install -Refer to the [root MOLA repository](https://github.com/MOLAorg/mola). - -## License -This package is released under the GNU GPL v3 license. Other options available upon request. diff --git a/mola_navstate_fuse/docs/module-mola-navstate-fuse.rst b/mola_navstate_fuse/docs/module-mola-navstate-fuse.rst deleted file mode 100644 index a35c5438..00000000 --- a/mola_navstate_fuse/docs/module-mola-navstate-fuse.rst +++ /dev/null @@ -1,12 +0,0 @@ -.. _mola-navstate-fuse: - -======================================== -Module: mola-navstate-fuse -======================================== - -Write me! - - -.. index:: - single: mola-navstate-fuse - module: mola-navstate-fuse diff --git a/mola_navstate_fuse/include/mola_navstate_fuse/NavStateFuse.h b/mola_navstate_fuse/include/mola_navstate_fuse/NavStateFuse.h deleted file mode 100644 index 10f321d7..00000000 --- a/mola_navstate_fuse/include/mola_navstate_fuse/NavStateFuse.h +++ /dev/null @@ -1,142 +0,0 @@ -/* ------------------------------------------------------------------------- - * A Modular Optimization framework for Localization and mApping (MOLA) - * - * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria - * Licensed under the GNU GPL v3 for non-commercial applications. - * - * This file is part of MOLA. - * MOLA is free software: you can redistribute it and/or modify it under the - * terms of the GNU General Public License as published by the Free Software - * Foundation, either version 3 of the License, or (at your option) any later - * version. - * - * MOLA is distributed in the hope that it will be useful, but WITHOUT ANY - * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR - * A PARTICULAR PURPOSE. See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along with - * MOLA. If not, see . - * ------------------------------------------------------------------------- */ -/** - * @file NavStateFuse.h - * @brief Fuse of odometry, IMU, and SE(3) pose/twist estimations. - * @author Jose Luis Blanco Claraco - * @date Jan 22, 2024 - */ -#pragma once - -#include -#include -#include -#include -#include -#include -#include - -#include - -namespace mola -{ -/** Fuse of odometry, IMU, and SE(3) pose/twist estimations. - * - * Usage: - * - (1) Call initialize() or set the required parameters directly in params_. - * - (2) Integrate measurements with `fuse_*()` methods. Each CObservation - * class includes a `timestamp` field which is used to estimate the - * trajectory. - * - (3) Repeat (2) as needed. - * - (4) Read the estimation up to any nearby moment in time with - * estimated_navstate() - * - * Old observations are automatically removed. - * - * \note This implementation of mola::NavStateFilter ignores the passed - * "frame_id" - * - * \sa IMUIntegrator - * \ingroup mola_imu_preintegration_grp - */ -class NavStateFuse : public mola::NavStateFilter - -{ - public: - NavStateFuse() = default; - ~NavStateFuse() = default; - - /** \name Main API - * @{ */ - - NavStateFuseParams params_; - - /** - * @brief Initializes the object and reads all parameters from a YAML node. - * @param cfg a YAML node with a dictionary of parameters to load from. - */ - void initialize(const mrpt::containers::yaml& cfg) override; - - /** Resets the estimator state to an initial state. - * \sa currentIntegrationState - */ - void reset() override; - - /** Integrates new SE(3) pose estimation of the vehicle wrt frame_id - */ - void fuse_pose( - const mrpt::Clock::time_point& timestamp, - const mrpt::poses::CPose3DPDFGaussian& pose, - const std::string& frame_id) override; - - /** Integrates new wheels-based odometry observations into the estimator. - * This is a convenience method that internally ends up calling - * fuse_pose(), but computing the uncertainty of odometry increments - * according to a given motion model. - */ - void fuse_odometry( - const mrpt::obs::CObservationOdometry& odom, - const std::string& odomName = "odom_wheels") override; - - /** Integrates new IMU observations into the estimator */ - void fuse_imu(const mrpt::obs::CObservationIMU& imu) override; - - /** Integrates new GNSS observations into the estimator */ - void fuse_gnss(const mrpt::obs::CObservationGPS& gps) override; - - /** Integrates new twist estimation (in the odom frame) */ - void fuse_twist( - const mrpt::Clock::time_point& timestamp, - const mrpt::math::TTwist3D& twist, - const mrpt::math::CMatrixDouble66& twistCov) override; - - /** Computes the estimated vehicle state at a given timestep using the - * observations in the time window. A std::nullopt is returned if there is - * no valid observations yet, or if requested a timestamp out of the model - * validity time window (e.g. too far in the future to be trustful). - */ - std::optional estimated_navstate( - const mrpt::Clock::time_point& timestamp, - const std::string& frame_id) override; - - std::optional get_last_twist() const - { - return state_.last_twist; - } - - /** @} */ - - private: - struct State - { - State() = default; - ~State() = default; - - std::optional last_odom_obs; - std::optional last_pose_obs_tim; - std::optional last_pose; - std::optional last_twist; - bool pose_already_updated_with_odom = false; - }; - - State state_; -}; - -} // namespace mola diff --git a/mola_navstate_fuse/include/mola_navstate_fuse/NavStateFuseParams.h b/mola_navstate_fuse/include/mola_navstate_fuse/NavStateFuseParams.h deleted file mode 100644 index af363c6e..00000000 --- a/mola_navstate_fuse/include/mola_navstate_fuse/NavStateFuseParams.h +++ /dev/null @@ -1,58 +0,0 @@ -/* ------------------------------------------------------------------------- - * A Modular Optimization framework for Localization and mApping (MOLA) - * - * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria - * Licensed under the GNU GPL v3 for non-commercial applications. - * - * This file is part of MOLA. - * MOLA is free software: you can redistribute it and/or modify it under the - * terms of the GNU General Public License as published by the Free Software - * Foundation, either version 3 of the License, or (at your option) any later - * version. - * - * MOLA is distributed in the hope that it will be useful, but WITHOUT ANY - * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR - * A PARTICULAR PURPOSE. See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along with - * MOLA. If not, see . - * ------------------------------------------------------------------------- */ -/** - * @file NavStateFuseParams.h - * @brief Parameters for NavStateFuse - * @author Jose Luis Blanco Claraco - * @date Jan 22, 2024 - */ - -#pragma once - -#include -#include -#include - -namespace mola -{ -/** Parameters needed by NavStateFuse. - * - * \ingroup mola_imu_preintegration_grp - */ -class NavStateFuseParams -{ - public: - NavStateFuseParams() = default; - ~NavStateFuseParams() = default; - - /// Loads all parameters from a YAML map node. - void loadFrom(const mrpt::containers::yaml& cfg); - - /** Valid estimations will be extrapolated only up to this time since the - * last incorporated observation. */ - double max_time_to_use_velocity_model = 2.0; // [s] - - mrpt::math::TTwist3D initial_twist; - - double sigma_random_walk_acceleration_linear = 1.0; // [m/s²] - double sigma_random_walk_acceleration_angular = 1.0; // [rad/s²] -}; - -} // namespace mola diff --git a/mola_navstate_fuse/package.xml b/mola_navstate_fuse/package.xml deleted file mode 100644 index c5a8dad6..00000000 --- a/mola_navstate_fuse/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - - - mola_navstate_fuse - 1.4.1 - SE(3) pose and twist path data fusion estimator - - Jose-Luis Blanco-Claraco - GPLv3 - - https://github.com/MOLAorg/mola/tree/develop/mola_navstate_fuse - - - mola_common - mola_kernel - mola_imu_preintegration - mrpt_libobs - - doxygen - - - cmake - - cmake - - - - diff --git a/mola_navstate_fuse/src/NavStateFuse.cpp b/mola_navstate_fuse/src/NavStateFuse.cpp deleted file mode 100644 index 32ea6806..00000000 --- a/mola_navstate_fuse/src/NavStateFuse.cpp +++ /dev/null @@ -1,190 +0,0 @@ -/* ------------------------------------------------------------------------- - * A Modular Optimization framework for Localization and mApping (MOLA) - * - * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria - * Licensed under the GNU GPL v3 for non-commercial applications. - * - * This file is part of MOLA. - * MOLA is free software: you can redistribute it and/or modify it under the - * terms of the GNU General Public License as published by the Free Software - * Foundation, either version 3 of the License, or (at your option) any later - * version. - * - * MOLA is distributed in the hope that it will be useful, but WITHOUT ANY - * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR - * A PARTICULAR PURPOSE. See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along with - * MOLA. If not, see . - * ------------------------------------------------------------------------- */ -/** - * @file NavStateFuse.cpp - * @brief Fuse of odometry, IMU, and SE(3) pose/twist estimations. - * @author Jose Luis Blanco Claraco - * @date Jan 22, 2024 - */ - -#include -#include -#include - -using namespace mola; - -void NavStateFuse::initialize(const mrpt::containers::yaml& cfg) -{ - reset(); - - // Load params: - params_.loadFrom(cfg); -} - -void NavStateFuse::reset() -{ - // reset: - state_ = State(); -} - -void NavStateFuse::fuse_odometry( - const mrpt::obs::CObservationOdometry& odom, - [[maybe_unused]] const std::string& odomName) -{ - // this will work well only for simple datasets with one odometry: - if (state_.last_odom_obs && state_.last_pose) - { - const auto poseIncr = odom.odometry - state_.last_odom_obs->odometry; - - state_.last_pose->mean = - state_.last_pose->mean + mrpt::poses::CPose3D(poseIncr); - - // We can skip velocity-based model, but retain the twist: - // state_.last_twist: do not modify - state_.pose_already_updated_with_odom = true; - } - // copy: - state_.last_odom_obs = odom; -} - -void NavStateFuse::fuse_imu(const mrpt::obs::CObservationIMU& imu) -{ - // TODO(jlbc) - (void)imu; -} - -void NavStateFuse::fuse_gnss(const mrpt::obs::CObservationGPS& gps) -{ - // TODO(jlbc) - (void)gps; -} - -void NavStateFuse::fuse_pose( - const mrpt::Clock::time_point& timestamp, - const mrpt::poses::CPose3DPDFGaussian& pose, - [[maybe_unused]] const std::string& frame_id) -{ - mrpt::poses::CPose3D incrPose; - - // numerical sanity: variances>=0 (==0 allowed for some components only) - for (int i = 0; i < 6; i++) ASSERT_GE_(pose.cov(i, i), .0); - // and the sum of all strictly >0 - ASSERT_GT_(pose.cov.trace(), .0); - - double dt = 0; - if (state_.last_pose_obs_tim) - dt = mrpt::system::timeDifference(*state_.last_pose_obs_tim, timestamp); - - if (dt < params_.max_time_to_use_velocity_model && state_.last_pose) - { - ASSERT_GT_(dt, .0); - - auto& tw = state_.last_twist.emplace(); - - incrPose = pose.mean - (state_.last_pose)->mean; - - tw.vx = incrPose.x() / dt; - tw.vy = incrPose.y() / dt; - tw.vz = incrPose.z() / dt; - - const auto logRot = - mrpt::poses::Lie::SO<3>::log(incrPose.getRotationMatrix()); - - tw.wx = logRot[0] / dt; - tw.wy = logRot[1] / dt; - tw.wz = logRot[2] / dt; - } - else { state_.last_twist.reset(); } - - // save for next iter: - state_.last_pose = pose; - state_.last_pose_obs_tim = timestamp; - state_.pose_already_updated_with_odom = false; -} - -void NavStateFuse::fuse_twist( - [[maybe_unused]] const mrpt::Clock::time_point& timestamp, - const mrpt::math::TTwist3D& twist, - [[maybe_unused]] const mrpt::math::CMatrixDouble66& twistCov) -{ - state_.last_twist = twist; -} - -std::optional NavStateFuse::estimated_navstate( - const mrpt::Clock::time_point& timestamp, - [[maybe_unused]] const std::string& frame_id) -{ - if (!state_.last_pose_obs_tim) return {}; // None - - const double dt = - mrpt::system::timeDifference(*state_.last_pose_obs_tim, timestamp); - - if (!state_.last_twist || !state_.last_pose || - std::abs(dt) > params_.max_time_to_use_velocity_model) - return {}; // None - - NavState ret; - - mrpt::poses::CPose3D poseExtrapolation; - - if (state_.pose_already_updated_with_odom) - { - // We have already updated the pose via wheels odometry, don't - // extrapolate: - poseExtrapolation = mrpt::poses::CPose3D::Identity(); - } - else - { // normal case: use twist to extrapolate: - - const auto& tw = state_.last_twist.value(); - - // For the velocity model, we don't have any known "bias": - const mola::RotationIntegrationParams rotParams = {}; - - const auto rot33 = - mola::incremental_rotation({tw.wx, tw.wy, tw.wz}, rotParams, dt); - - poseExtrapolation = mrpt::poses::CPose3D::FromRotationAndTranslation( - rot33, mrpt::math::TVector3D(tw.vx, tw.vy, tw.vz) * dt); - } - - // pose mean: - ret.pose.mean = state_.last_pose->mean + poseExtrapolation; - - // pose cov: - auto cov = state_.last_pose->cov; - - double varXYZ = - mrpt::square(dt * params_.sigma_random_walk_acceleration_linear); - double varRot = - mrpt::square(dt * params_.sigma_random_walk_acceleration_angular); - - for (int i = 0; i < 3; i++) cov(i, i) += varXYZ; - for (int i = 3; i < 6; i++) cov(i, i) += varRot; - - ret.pose.cov_inv = cov.inverse_LLt(); - - // twist: - ret.twist = state_.last_twist.value(); - - // TODO(jlbc): twist covariance - - return ret; -} diff --git a/mola_navstate_fuse/src/NavStateFuseParams.cpp b/mola_navstate_fuse/src/NavStateFuseParams.cpp deleted file mode 100644 index 8409c0b5..00000000 --- a/mola_navstate_fuse/src/NavStateFuseParams.cpp +++ /dev/null @@ -1,48 +0,0 @@ -/* ------------------------------------------------------------------------- - * A Modular Optimization framework for Localization and mApping (MOLA) - * - * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria - * Licensed under the GNU GPL v3 for non-commercial applications. - * - * This file is part of MOLA. - * MOLA is free software: you can redistribute it and/or modify it under the - * terms of the GNU General Public License as published by the Free Software - * Foundation, either version 3 of the License, or (at your option) any later - * version. - * - * MOLA is distributed in the hope that it will be useful, but WITHOUT ANY - * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR - * A PARTICULAR PURPOSE. See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along with - * MOLA. If not, see . - * ------------------------------------------------------------------------- */ -/** - * @file IMUIntegrationParams.cpp - * @brief Parameters for IMU preintegration. - * @author Jose Luis Blanco Claraco - * @date Sep 19, 2021 - */ - -#include - -using namespace mola; - -void NavStateFuseParams::loadFrom(const mrpt::containers::yaml& cfg) -{ - MCP_LOAD_REQ(cfg, max_time_to_use_velocity_model); - - MCP_LOAD_OPT(cfg, sigma_random_walk_acceleration_linear); - MCP_LOAD_OPT(cfg, sigma_random_walk_acceleration_angular); - - if (cfg.has("initial_twist")) - { - ASSERT_( - cfg["initial_twist"].isSequence() && - cfg["initial_twist"].asSequence().size() == 6); - - auto& tw = initial_twist; - const auto seq = cfg["initial_twist"].asSequenceRange(); - for (size_t i = 0; i < 6; i++) tw[i] = seq.at(i).as(); - } -} diff --git a/mola_navstate_fuse/src/register.cpp b/mola_navstate_fuse/src/register.cpp deleted file mode 100644 index cf1ab8c6..00000000 --- a/mola_navstate_fuse/src/register.cpp +++ /dev/null @@ -1,35 +0,0 @@ -/* ------------------------------------------------------------------------- - * A Modular Optimization framework for Localization and mApping (MOLA) - * - * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria - * Licensed under the GNU GPL v3 for non-commercial applications. - * - * This file is part of MOLA. - * MOLA is free software: you can redistribute it and/or modify it under the - * terms of the GNU General Public License as published by the Free Software - * Foundation, either version 3 of the License, or (at your option) any later - * version. - * - * MOLA is distributed in the hope that it will be useful, but WITHOUT ANY - * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR - * A PARTICULAR PURPOSE. See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along with - * MOLA. If not, see . - * ------------------------------------------------------------------------- */ - -/** - * @file register.cpp - * @brief Register RTTI classes - * @author Jose Luis Blanco Claraco - * @date Sep 18, 2021 - */ - -#include - -// using namespace mola; - -MRPT_INITIALIZER(do_register_navstate_fuse) -{ - // MOLA_REGISTER_MODULE(xx); -}