Merge pull request #29 from dic-iit/feature/vio

Add IMUPreintegrator advanceable and make ImageProcessor as advanceable object

CHANGELOG.md

@@ -9,9 +9,13 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 ### Fixed
 - Fixed implementation of `PointsTracker` class (https://github.com/dic-iit/kindyn-vio/pull/23).
 
+### Changed
+- Modified `ImageProcessor` class into a `BipedalLocomotion::System::Advanceable` type. (https://github.com/dic-iit/kindyn-vio/pull/29)
+
 ### Added
 - Implement `PerceptionCameraModels` library containing `PinHoleCamera` class.
 - Implement `PerceptionFeatures` library containing `PointsTracker` class and `ImageProcessor` class.
 - Add `ArucoDetectorExample` and `PointsTrackerTest` (https://github.com/dic-iit/kindyn-vio/pull/21).
 - Implement `LinesTracker` class in `PerceptionFeatures` library and add `LinesTrackerTest`. (https://github.com/dic-iit/kindyn-vio/pull/23).
 - Improve `ImageProcessor` class to consider `LinesTracker` class (https://github.com/dic-iit/kindyn-vio/pull/23).
+- Add `IMUPreintegrator` class into `Estimators` library. This class wraps GTSAM `CombinedIMUFactor` preintegration into a `BipedalLocomotion::System::Advanceable` type. (https://github.com/dic-iit/kindyn-vio/pull/29)

src/CMakeLists.txt

@@ -3,4 +3,5 @@
 # GNU Lesser General Public License v2.1 or any later version.
 
 add_subdirectory(Perception)
+add_subdirectory(Estimators)
 

src/Estimators/CMakeLists.txt

@@ -0,0 +1,13 @@
+# Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+# This software may be modified and distributed under the terms of the
+# GNU Lesser General Public License v2.1 or any later version.
+
+set(H_PREFIX include/KinDynVIO/Estimators)
+add_kindyn_vio_library(
+NAME                   Estimators
+SOURCES                src/States.cpp src/IMUPreintegrator.cpp
+PUBLIC_HEADERS         ${H_PREFIX}/States.h ${H_PREFIX}/IMUPreintegrator.h
+SUBDIRECTORIES         tests
+PUBLIC_LINK_LIBRARIES  BipedalLocomotion::ParametersHandler BipedalLocomotion::TextLogging BipedalLocomotion::Math gtsam gtsam_unstable metis-gtsam
+INSTALLATION_FOLDER    Estimators)
+

src/Estimators/include/KinDynVIO/Estimators/IMUPreintegrator.h

@@ -0,0 +1,132 @@
+/**
+ * @file IMUPreintegrator.h
+ * @authors Prashanth Ramadoss
+ * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
+ */
+
+#ifndef KINDYNVIO_ESTIMATORS_IMU_PREINTEGRATOR_H
+#define KINDYNVIO_ESTIMATORS_IMU_PREINTEGRATOR_H
+#include <BipedalLocomotion/System/Advanceable.h>
+#include <KinDynVIO/Estimators/States.h>
+// gtsam includes
+#include <gtsam/navigation/CombinedImuFactor.h>
+#include <gtsam/inference/Key.h>
+
+#include <Eigen/Dense>
+#include <memory>
+
+namespace KinDynVIO
+{
+namespace Estimators
+{
+
+struct IMUPreintegratorInput
+{
+    double ts;
+    Eigen::Vector3d linAcc; // m per second per second
+    Eigen::Vector3d gyro; // radians per second
+
+    gtsam::Key posei, posej; // IMU poses at ith and jth timestep
+    gtsam::Key vi, vj; // IMU linear velocities at ith and jth timestep
+    gtsam::Key bi, bj; // IMU biases at ith and jth timestep
+};
+
+template <typename PreintegratedFactor>
+class IMUPreintegrator : public BipedalLocomotion::System::Advanceable<IMUPreintegratorInput, PreintegratedFactor>
+{
+public:
+    virtual bool initialize(std::weak_ptr<const BipedalLocomotion::ParametersHandler::IParametersHandler> handler)
+    {
+        return true;
+    }
+
+    virtual bool setInput(const IMUPreintegratorInput& input) = 0;
+
+    virtual bool advance() = 0;
+    virtual const PreintegratedFactor& getOutput() const = 0;
+    virtual bool isOutputValid() const = 0;
+
+    virtual bool getPredictedState(const IMUState& currentState,
+                                   IMUState& predictedState) = 0;
+    virtual void resetIMUIntegration() = 0;
+};
+
+
+
+/*
+ * An advanceable block for IMU measurements preintegration
+ *
+ * This class uses the GTSAM's PreintegratedMeasurements class,
+ * which can be used in two ways depending on how GTSAM is compiled,
+ * 1. Manifold preintegration (described in [4])
+ * 2. Tangent preintegration (check gtsam/doc/ImuFactor.pdf) [default since GTSAM 4.0]
+ * Please note the documentation in https://gtsam.org/notes/IMU-Factor.html
+ * We have decided to call this class ForsterIMUPreintegrator in general.
+ *
+ * If you are using the factor, please cite:
+ * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating
+ * conditionally independent sets in factor graphs: a unifying perspective based
+ * on smart factors, Int. Conf. on Robotics and Automation (ICRA), 2014.
+ *
+ * REFERENCES:
+ * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups",
+ *     Volume 2, 2008.
+ * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for
+ *     High-Dynamic Motion in Built Environments Without Initial Conditions",
+ *     TRO, 28(1):61-76, 2012.
+ * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor:
+ *     Computation of the Jacobian Matrices", Tech. Report, 2013.
+ * [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, IMU Preintegration on
+ *     Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation,
+ *     Robotics: Science and Systems (RSS), 2015.
+ */
+class ForsterIMUPreintegrator : public IMUPreintegrator<gtsam::CombinedImuFactor>
+{
+public:
+    ForsterIMUPreintegrator();
+    virtual ~ForsterIMUPreintegrator();
+
+    /**
+     * Initialize the ForsterIMUPreintegrator.
+     * @param paramHandler pointer to the parameters handler.
+     * @note the following parameters are required by the class
+     * | Parameter Name        |        Type        |                     Description                                         | Mandatory |
+     * |:---------------------:|:------------------:|:-----------------------------------------------------------------------:|:---------:|
+     * |       `sigma_acc`     |      `double`      | Isotropic standard deviation of accelerometer noise in continuous time. |    No     |
+     * |       `sigma_gyro`    |      `double`      | Isotropic standard deviation of gyroscope noise in continuous time.     |    No     |
+     * |       `sigma_b_acc`   |      `double`      | Isotropic standard deviation of accelerometer bias in continuous time.  |    No     |
+     * |       `sigma_b_gyro`  |      `double`      | Isotropic standard deviation of gyroscope bias in continuous time.      |    No     |
+     * |`sigma_pos_integration`|      `double`      | Isotropic standard deviation for position integration from velocities   |    No     |
+     * |       `error_bias`    |      `double`      |             Error for IMU bias in preintegration.                       |    No     |
+     * |       `initial_bias`  |`vector of double`  |                      Initial bias values.                               |    No     |
+     * |       `gravity`       |`vector of double`  |           Acceleration due to gravity in the inertial frame.            |    No     |
+     */
+    bool initialize(std::weak_ptr<const BipedalLocomotion::ParametersHandler::IParametersHandler> handler) final;
+    bool setInput(const IMUPreintegratorInput& input) final;
+    virtual bool advance() final;
+
+
+    // the following functions are all intended to be called
+    // only after multiple calls to advance(),
+    // relevant to the intermediate integration steps
+    // between states at i-th and j-th timestamp, has been called
+    // currently no internal check is available for this
+    // and the user needs to be careful about these function calls
+    // depending on the satisfaction of some conditional statements
+    const gtsam::CombinedImuFactor& getOutput() const final;
+    bool isOutputValid() const final;
+
+    virtual bool getPredictedState(const IMUState& currentState,
+                                   IMUState& predictedState) final;
+    virtual void resetIMUIntegration() final;
+    virtual void resetIMUIntegration(const gtsam::imuBias::ConstantBias& bias);
+
+private:
+    class Impl;
+    std::unique_ptr<Impl> m_pimpl;
+};
+
+} // namespace Estimators
+} // namespace KinDynVIO
+#endif // KINDYNVIO_ESTIMATORS_SMOOTHER_H

src/Estimators/include/KinDynVIO/Estimators/States.h

@@ -0,0 +1,52 @@
+/**
+ * @file States.h
+ * @authors Prashanth Ramadoss
+ * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
+ */
+
+#ifndef KINDYNVIO_ESTIMATORS_STATES_H
+#define KINDYNVIO_ESTIMATORS_STATES_H
+
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/navigation/ImuBias.h>
+
+namespace KinDynVIO
+{
+namespace Estimators
+{
+
+class IMUState
+{
+public:
+    IMUState();
+    IMUState(const IMUState& other);
+    IMUState(const gtsam::Pose3& pose,
+             const gtsam::Vector3& velocity,
+             const gtsam::imuBias::ConstantBias& bias);
+
+    void setPose(const gtsam::Pose3& pose);
+    void setLinearVelocity(const gtsam::Vector3& velocity);
+    void setBias(const gtsam::imuBias::ConstantBias& bias);
+
+    const gtsam::Pose3& pose() const;
+    gtsam::Vector3 p() const;
+    gtsam::Quaternion quat() const;
+    gtsam::Vector3 rpy() const;
+    const gtsam::Vector3& v() const;
+    const gtsam::imuBias::ConstantBias& b() const;
+    const gtsam::Vector3& ba() const;
+    const gtsam::Vector3& bg() const;
+
+    void print() const;
+
+private:
+    gtsam::Pose3 m_pose;                 // Pose of IMU wrt inertial
+    gtsam::Vector3 m_linearVelocity;     // mixed trivialized linear velocity of IMU wrt inertial
+    gtsam::imuBias::ConstantBias m_bias; // IMU Bias
+};
+
+} // namespace Estimators
+} // namespace KinDynVIO
+
+#endif // KINDYNVIO_ESTIMATORS_KINDYNVIO_STATES_H

src/Estimators/src/IMUPreintegrator.cpp

@@ -0,0 +1,161 @@
+/**
+ * @file IMUPreintegrator.cpp
+ * @authors Prashanth Ramadoss
+ * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
+ */
+
+#include <BipedalLocomotion/Math/Constants.h>
+#include <BipedalLocomotion/TextLogging/Logger.h>
+#include <KinDynVIO/Estimators/IMUPreintegrator.h>
+
+using namespace KinDynVIO::Estimators;
+
+class ForsterIMUPreintegrator::Impl
+{
+public:
+    IMUPreintegratorInput input;
+    boost::shared_ptr<gtsam::PreintegratedCombinedMeasurements::Params> params; // requires to be
+                                                                                // boost shared_ptr
+                                                                                // for use in
+                                                                                // constructor of
+                                                                                // imuPreInt
+    std::shared_ptr<gtsam::PreintegratedCombinedMeasurements> imuPreInt;
+    gtsam::CombinedImuFactor imuFactor;
+
+    bool initialized{false};
+    double prevTime;
+
+    // param placeholders
+    double sigmaAcc{1e-2}, sigmaGyro{1e-3};
+    double sigmaBiasAcc{1e-3}, sigmaBiasGyro{1e-3};
+    double sigmaPosIntegration, errorBias{1e-5};
+    Eigen::Matrix<double, 6, 1> biasInitial;
+    Eigen::Vector3d gravity;
+    Eigen::Matrix3d I3;
+    Eigen::Matrix<double, 6, 6> I6;
+};
+
+ForsterIMUPreintegrator::ForsterIMUPreintegrator()
+    : m_pimpl(std::make_unique<ForsterIMUPreintegrator::Impl>())
+{
+    m_pimpl->biasInitial.setZero();
+    m_pimpl->gravity << 0, 0, -BipedalLocomotion::Math::StandardAccelerationOfGravitation;
+    m_pimpl->I3.setIdentity();
+    m_pimpl->I6.setIdentity();
+}
+
+ForsterIMUPreintegrator::~ForsterIMUPreintegrator()
+{
+}
+
+bool ForsterIMUPreintegrator::initialize(
+    std::weak_ptr<const BipedalLocomotion::ParametersHandler::IParametersHandler> handler)
+{
+    const std::string printPrefix{"[ForsterIMUPreintegrator::initialize]"};
+    auto handle = handler.lock();
+    if (handle == nullptr)
+    {
+        BipedalLocomotion::log()->error("{} The parameter handler has expired. "
+                                        "Please check its scope.",
+                                        printPrefix);
+        return false;
+    }
+
+    handle->getParameter("sigma_acc", m_pimpl->sigmaAcc);
+    handle->getParameter("sigma_gyro", m_pimpl->sigmaGyro);
+    handle->getParameter("sigma_b_acc", m_pimpl->sigmaBiasAcc);
+    handle->getParameter("sigma_b_gyro", m_pimpl->sigmaBiasGyro);
+    handle->getParameter("sigma_pos_integration", m_pimpl->sigmaPosIntegration);
+    handle->getParameter("error_bias", m_pimpl->errorBias);
+    handle->getParameter("initial_bias", m_pimpl->biasInitial);
+    handle->getParameter("gravity", m_pimpl->gravity);
+
+    m_pimpl->params
+        = boost::make_shared<gtsam::PreintegratedCombinedMeasurements::Params>(m_pimpl->gravity);
+    m_pimpl->params->setAccelerometerCovariance(m_pimpl->I3 * m_pimpl->sigmaAcc
+                                                * m_pimpl->sigmaAcc);
+    m_pimpl->params->setGyroscopeCovariance(m_pimpl->I3 * m_pimpl->sigmaGyro * m_pimpl->sigmaGyro);
+    m_pimpl->params->setBiasAccCovariance(m_pimpl->I3 * m_pimpl->sigmaBiasAcc
+                                          * m_pimpl->sigmaBiasAcc);
+    m_pimpl->params->setBiasOmegaCovariance(m_pimpl->I3 * m_pimpl->sigmaBiasGyro
+                                            * m_pimpl->sigmaBiasGyro);
+    m_pimpl->params->setIntegrationCovariance(m_pimpl->I3 * m_pimpl->sigmaPosIntegration
+                                              * m_pimpl->sigmaPosIntegration);
+    m_pimpl->params->setBiasAccOmegaInt(m_pimpl->I6 * m_pimpl->errorBias);
+
+    // force coriolis effect to false
+    m_pimpl->params->setUse2ndOrderCoriolis(false);
+
+    gtsam::imuBias::ConstantBias bias(m_pimpl->biasInitial);
+    m_pimpl->imuPreInt
+        = std::make_shared<gtsam::PreintegratedCombinedMeasurements>(m_pimpl->params, bias);
+
+    m_pimpl->initialized = true;
+    return true;
+}
+
+bool ForsterIMUPreintegrator::setInput(const IMUPreintegratorInput& input)
+{
+    std::string printPrefix{"[ForsterIMUPreintegrator::setInput]"};
+    if (!m_pimpl->initialized)
+    {
+        BipedalLocomotion::log()->error("{} Please call initialize first", printPrefix);
+        return false;
+    }
+
+    m_pimpl->input = input;
+    return true;
+}
+
+bool ForsterIMUPreintegrator::advance()
+{
+    const auto& currTime = m_pimpl->input.ts;
+    const Eigen::Vector3d& acc = m_pimpl->input.linAcc;
+    const Eigen::Vector3d& gyro = m_pimpl->input.gyro;
+    double dt{currTime - m_pimpl->prevTime};
+
+    m_pimpl->imuPreInt->integrateMeasurement(acc, gyro, dt);
+    m_pimpl->prevTime = m_pimpl->input.ts;
+    return true;
+}
+
+bool ForsterIMUPreintegrator::isOutputValid() const
+{
+    return m_pimpl->initialized;
+}
+
+void ForsterIMUPreintegrator::resetIMUIntegration(const gtsam::imuBias::ConstantBias& bias)
+{
+    m_pimpl->imuPreInt->resetIntegrationAndSetBias(bias);
+}
+
+void ForsterIMUPreintegrator::resetIMUIntegration()
+{
+    m_pimpl->imuPreInt->resetIntegration();
+}
+
+const gtsam::CombinedImuFactor& ForsterIMUPreintegrator::getOutput() const
+{
+    const auto& Xi = m_pimpl->input.posei;
+    const auto& Xj = m_pimpl->input.posei;
+    const auto& vi = m_pimpl->input.vi;
+    const auto& vj = m_pimpl->input.vj;
+    const auto& bi = m_pimpl->input.bi;
+    const auto& bj = m_pimpl->input.bj;
+
+    m_pimpl->imuFactor = gtsam::CombinedImuFactor(Xi, vi, Xj, vj, bi, bj, *m_pimpl->imuPreInt);
+    return m_pimpl->imuFactor;
+}
+
+bool ForsterIMUPreintegrator::getPredictedState(const IMUState& currentState,
+                                                IMUState& predictedState)
+{
+    gtsam::NavState navStatePred
+        = m_pimpl->imuPreInt->predict(gtsam::NavState(currentState.pose(), currentState.v()),
+                                      currentState.b());
+    predictedState.setPose(navStatePred.pose());
+    predictedState.setLinearVelocity(navStatePred.v());
+    predictedState.setBias(currentState.b());
+    return true;
+}