-
Notifications
You must be signed in to change notification settings - Fork 871
GalileanFactor ! #2231
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Draft
dellaert
wants to merge
7
commits into
develop
Choose a base branch
from
feature/GalileanImuFactor
base: develop
Could not load branches
Branch not found: {{ refName }}
Loading
Could not load tags
Nothing to show
Loading
Are you sure you want to change the base?
Some commits from the old base branch may be removed from the timeline,
and old review comments may become outdated.
Draft
GalileanFactor ! #2231
Changes from all commits
Commits
Show all changes
7 commits
Select commit
Hold shift + click to select a range
a0592a6
Fix Boost
dellaert 2592f18
Fix badges
dellaert 9c6fcd9
Fix boost on windows
dellaert a198644
Move batch-integrate method to base
dellaert 18ea6d7
Add stubbed GalileanImuFactor
dellaert 02b578e
Make tests fail :-)
dellaert cb46dde
Add preintegration on Gal(3) (W.I.P.)
giuliodelama File filter
Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
There are no files selected for viewing
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,56 +1,88 @@ | ||
| ############################################################################### | ||
| # Find boost | ||
|
|
||
| # To change the path for boost, you will need to set: | ||
| # BOOST_ROOT: path to install prefix for boost | ||
| # Boost_NO_SYSTEM_PATHS: set to true to keep the find script from ignoring BOOST_ROOT | ||
| ################################################################################ | ||
| # Find and configure the Boost libraries. | ||
| # | ||
| # To customize the Boost installation path, you can set the following variables | ||
| # when running CMake: | ||
| # - BOOST_ROOT: Path to the Boost installation prefix. | ||
| # - BOOST_INCLUDEDIR: Path to the Boost include directory. | ||
| # - BOOST_LIBRARYDIR: Path to the Boost library directory. | ||
| # | ||
| # These hints are particularly important for manual installations on Windows. | ||
| ################################################################################ | ||
|
|
||
| if(MSVC) | ||
| # By default, boost only builds static libraries on windows | ||
| set(Boost_USE_STATIC_LIBS ON) # only find static libs | ||
| # If we ever reset above on windows and, ... | ||
| # If we use Boost shared libs, disable auto linking. | ||
| # Some libraries, at least Boost Program Options, rely on this to export DLL symbols. | ||
| # By default, Boost pre-compiled binaries for Windows are static libraries. | ||
| set(Boost_USE_STATIC_LIBS ON) | ||
| if(NOT Boost_USE_STATIC_LIBS) | ||
| list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC BOOST_ALL_NO_LIB BOOST_ALL_DYN_LINK) | ||
| endif() | ||
| # Virtual memory range for PCH exceeded on VS2015 | ||
| if(MSVC_VERSION LESS 1910) # older than VS2017 | ||
| list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE -Zm295) | ||
| endif() | ||
| endif() | ||
|
|
||
| # --- GUIDANCE FOR LOCAL WINDOWS DEVELOPMENT --- | ||
| # | ||
| # If you enable Boost features on Windows, there are two primary ways to provide | ||
| # the Boost libraries: | ||
| # | ||
| # 1. RECOMMENDED: Use a package manager like vcpkg. | ||
| # - Vcpkg (https://github.com/microsoft/vcpkg) handles the download, build, | ||
| # and integration of Boost. It provides modern CMake config files and works | ||
| # seamlessly with this script without any special configuration. | ||
| # | ||
| # 2. ALTERNATIVE: Manual Installation (e.g., pre-compiled binaries). | ||
| # - If you download pre-compiled binaries, you MUST provide hints to CMake so | ||
| # it can find your installation. At a minimum, set BOOST_ROOT, e.g.: | ||
| # cmake .. -DBOOST_ROOT=C:/local/boost_1_87_0 | ||
| # | ||
| ################################################################################ | ||
|
|
||
| # Store these in variables so they are automatically replicated in GTSAMConfig.cmake and such. | ||
| set(BOOST_FIND_MINIMUM_VERSION 1.65) | ||
| set(BOOST_FIND_MINIMUM_COMPONENTS serialization system filesystem thread program_options date_time timer chrono regex) | ||
| # Set minimum required Boost version and components. | ||
| # NOTE: "system" is intentionally omitted. It is a transitive dependency of other | ||
| # components (like filesystem) and will be found automatically. Explicitly | ||
| # requesting it can cause issues with modern header-only versions of Boost. | ||
| set(BOOST_FIND_MINIMUM_VERSION 1.70) | ||
| set(BOOST_FIND_MINIMUM_COMPONENTS serialization filesystem thread program_options date_time timer chrono regex) | ||
|
|
||
| find_package(Boost ${BOOST_FIND_MINIMUM_VERSION} COMPONENTS ${BOOST_FIND_MINIMUM_COMPONENTS} REQUIRED) | ||
| # Find the Boost package. On systems with modern installations (vcpkg, Homebrew), | ||
| # this will use CMake's "Config mode". With manual installations (especially on | ||
| # Windows), providing the hints above will trigger the legacy "Module mode". | ||
| find_package(Boost ${BOOST_FIND_MINIMUM_VERSION} REQUIRED | ||
| COMPONENTS ${BOOST_FIND_MINIMUM_COMPONENTS} | ||
| ) | ||
|
|
||
| # Required components | ||
| if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_SYSTEM_LIBRARY OR NOT Boost_FILESYSTEM_LIBRARY OR | ||
| NOT Boost_THREAD_LIBRARY OR NOT Boost_DATE_TIME_LIBRARY) | ||
| message(FATAL_ERROR "Missing required Boost components >= v1.65, please install/upgrade Boost or configure your search paths.") | ||
| endif() | ||
| # Verify that the required Boost component targets were successfully found and imported. | ||
| foreach(_t IN ITEMS Boost::serialization Boost::filesystem Boost::thread Boost::date_time) | ||
| if(NOT TARGET ${_t}) | ||
| message(FATAL_ERROR "Missing required Boost component target: ${_t}. Please install/upgrade Boost or set BOOST_ROOT/Boost_DIR correctly.") | ||
| endif() | ||
| endforeach() | ||
|
|
||
| option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF) | ||
| # Allow for not using the timer libraries on boost < 1.48 (GTSAM timing code falls back to old timer library) | ||
|
|
||
| set(GTSAM_BOOST_LIBRARIES | ||
| Boost::serialization | ||
| Boost::system | ||
| Boost::filesystem | ||
| Boost::thread | ||
| Boost::date_time | ||
| Boost::regex | ||
| ) | ||
| if (GTSAM_DISABLE_NEW_TIMERS) | ||
| message("WARNING: GTSAM timing instrumentation manually disabled") | ||
| list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC DGTSAM_DISABLE_NEW_TIMERS) | ||
|
|
||
| if(GTSAM_DISABLE_NEW_TIMERS) | ||
| message("WARNING: GTSAM timing instrumentation manually disabled") | ||
| list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC DGTSAM_DISABLE_NEW_TIMERS) | ||
| else() | ||
| if(Boost_TIMER_LIBRARY) | ||
| list(APPEND GTSAM_BOOST_LIBRARIES Boost::timer Boost::chrono) | ||
| # Link against compiled timer libraries if they exist. | ||
| if(TARGET Boost::timer AND TARGET Boost::chrono) | ||
| list(APPEND GTSAM_BOOST_LIBRARIES Boost::timer Boost::chrono) | ||
| else() | ||
| # Fallback for header-only timer: link librt on Linux. | ||
| if(UNIX AND NOT APPLE) | ||
| list(APPEND GTSAM_BOOST_LIBRARIES rt) | ||
| message("WARNING: Using header-only Boost timer; adding -lrt on Linux.") | ||
| else() | ||
| list(APPEND GTSAM_BOOST_LIBRARIES rt) # When using the header-only boost timer library, need -lrt | ||
| message("WARNING: GTSAM timing instrumentation will use the older, less accurate, Boost timer library because boost older than 1.48 was found.") | ||
| message("WARNING: Using header-only Boost timer; no extra libs required on this platform.") | ||
| endif() | ||
| endif() | ||
| endif() | ||
| endif() |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,91 @@ | ||
| /* ---------------------------------------------------------------------------- | ||
|
|
||
| * GTSAM Copyright 2010, Georgia Tech Research Corporation, | ||
| * Atlanta, Georgia 30332-0415 | ||
| * All Rights Reserved | ||
| * Authors: Frank Dellaert, et al. (see THANKS for the full author list) | ||
|
|
||
| * See LICENSE for the license information | ||
|
|
||
| * -------------------------------------------------------------------------- */ | ||
|
|
||
| /** | ||
| * @file GalileanImuFactor.cpp | ||
| * @brief Implementation of Delama et al. IMU Factor | ||
| * @author Frank Dellaert | ||
| * @author Giulio Delama | ||
| */ | ||
|
|
||
| #include <gtsam/navigation/GalileanImuFactor.h> | ||
|
|
||
| namespace gtsam { | ||
|
|
||
| // -- Constructors ------------------------------------------------------------ | ||
| PreintegratedImuMeasurementsG::PreintegratedImuMeasurementsG( | ||
| const std::shared_ptr<Params>& p, const imuBias::ConstantBias& biasHat) | ||
| : Base(p, biasHat), p_(p) {} | ||
|
|
||
| // -- Integration API --------------------------------------------------------- | ||
| void PreintegratedImuMeasurementsG::resetIntegration() { | ||
| preintMatrix_ = Gal3().Identity(); | ||
| preintMeasCov_.setZero(); | ||
| preintBiasJacobian_.setZero(); | ||
| } | ||
|
|
||
| void PreintegratedImuMeasurementsG::integrateMeasurement( | ||
| const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) { | ||
| // Correct measurements for bias and construct input | ||
| Vector3 acc = biasHat_.correctAccelerometer(measuredAcc); | ||
| Vector3 omega = biasHat_.correctGyroscope(measuredOmega); | ||
| if (p_->body_P_sensor) { | ||
| const auto& bRs = p_->body_P_sensor->rotation(); | ||
| acc = bRs * acc; | ||
| omega = bRs * omega; | ||
| } | ||
| Vector10 wInput = Vector10::Zero(); | ||
| wInput.head<3>() = omega; | ||
| wInput.segment<3>(3) = acc; | ||
| wInput(9) = 1.0; // time scale | ||
| Matrix10 Jl; | ||
| Gal3 wExp = Gal3::Expmap(-wInput * dt, Jl).inverse(); | ||
| Matrix10 preintMatrixAdjoint = preintMatrix_.AdjointMap(); | ||
|
|
||
| // Propagate mean - from eq. (22) in the paper | ||
| preintMatrix_ = preintMatrix_.compose(wExp); | ||
|
|
||
| // Propagate covariance - from eq. (35) in the paper | ||
| // IMPORTANT: the covariance propagation is based on the Right-Invariant (RI) | ||
| // error since the equivariant error (34) is by definition RI. If we want to | ||
| // use the Left-Invariant (LI) error to use NavState in the error formulation | ||
| // for the factor, we need to change the covariance propagation accordingly | ||
| // with the Adjoint. For RI error, the A_ matrix is identity. | ||
| Matrix10 B_ = preintMatrixAdjoint * Jl * dt; | ||
| Matrix10 Qd = Matrix10::Zero(); | ||
| Qd.block<3, 3>(0, 0) = p_->gyroscopeCovariance / dt; | ||
| Qd.block<3, 3>(3, 3) = p_->accelerometerCovariance / dt; | ||
| preintMeasCov_.noalias() += B_ * Qd * B_.transpose(); | ||
|
|
||
| // Propagate bias Jacobian - from eq. (38) in the paper | ||
| Matrix20 phiBiasJacobian = Matrix20::Identity(); | ||
| phiBiasJacobian.block<10, 10>(9, 9) = -preintMatrixAdjoint * Jl * dt; | ||
| preintBiasJacobian_ = phiBiasJacobian * preintBiasJacobian_; | ||
| } | ||
|
|
||
| // -- print / equals ---------------------------------------------------------- | ||
| void PreintegratedImuMeasurementsG::print(const std::string& s) const { | ||
| std::cout << (s.empty() ? s : s + "\n") | ||
| << "PreintegratedImuMeasurementsG:" << std::endl; | ||
| std::cout << " preintMatrix_: " << preintMatrix_ << std::endl; | ||
| std::cout << " preintMeasCov_: \n" << preintMeasCov_ << std::endl; | ||
| std::cout << " preintBiasJacobian_: \n" << preintBiasJacobian_ << std::endl; | ||
| std::cout << " biasHat_: " << biasHat_.vector().transpose() << std::endl; | ||
| } | ||
| bool PreintegratedImuMeasurementsG::equals( | ||
| const PreintegratedImuMeasurementsG& o, double tol) const { | ||
| return p_->equals(*o.p_, tol) && biasHat_.equals(o.biasHat_, tol) && | ||
| preintMatrix_.equals(o.preintMatrix_, tol) && | ||
| equal_with_abs_tol(preintMeasCov_, o.preintMeasCov_, tol) && | ||
| equal_with_abs_tol(preintBiasJacobian_, o.preintBiasJacobian_, tol); | ||
| } | ||
|
|
||
| } // namespace gtsam | ||
Oops, something went wrong.
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Are these outdated now? I think you switched to LI?