ImuTypes.h

#pragma once
#include <NDEVR/Buffer.h>
#include <NDEVR/Vertex.h>
#if _WIN32
#include <utility>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <OrbSLAM/Thirdparty/Sophus/sophus/se3.hpp>
#include <mutex>
typedef float g_type; 
namespace NDEVR
{
    namespace IMU
    {
        static constexpr g_type GRAVITY_VALUE = 9.81f; 

        class Point
        {
        public:
            Point()
            {}
            Point(const Ray<3, fltp04>& acc, const Ray<3, fltp04>& gyro, fltp08 timestamp)
                : acc(cast<g_type>(acc[X]), cast<g_type>(acc[Y]), cast<g_type>(acc[Z]))
                , w(cast<g_type>(gyro[X]), cast<g_type>(gyro[Y]), cast<g_type>(gyro[Z]))
                , t(timestamp)
            {}
            Point(const Ray<3, fltp08>& acc, const Ray<3, fltp08>& gyro, fltp08 timestamp)
                : acc(cast<g_type>(acc[X]), cast<g_type>(acc[Y]), cast<g_type>(acc[Z]))
                , w(cast<g_type>(gyro[X]), cast<g_type>(gyro[Y]), cast<g_type>(gyro[Z]))
                , t(timestamp)
            {
            }
            Point(g_type acc_x, g_type acc_y, g_type acc_z,
                g_type ang_vel_x, g_type ang_vel_y, g_type ang_vel_z,
                fltp08 timestamp)
                : acc(acc_x, acc_y, acc_z)
                , w(ang_vel_x, ang_vel_y, ang_vel_z)
                , t(timestamp)
            {}
            template<class t_xyz_type>
            Point(const t_xyz_type Acc, const t_xyz_type Gyro, const double& timestamp)
                : acc(Acc.x, Acc.y, Acc.z)
                , w(Gyro.x, Gyro.y, Gyro.z)
                , t(timestamp)
            {}
        public:
            Eigen::Vector3<g_type> acc; 
            Eigen::Vector3<g_type> w; 
            fltp08 t = 0.0; 
            EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        };

        class Bias
        {
        public:
            Bias() :bax(0), bay(0), baz(0), bwx(0), bwy(0), bwz(0) {}
            Bias(const fltp04& b_acc_x, const fltp04& b_acc_y, const fltp04& b_acc_z,
                const fltp04& b_ang_vel_x, const fltp04& b_ang_vel_y, const fltp04& b_ang_vel_z)
                : bax(cast<g_type>(b_acc_x))
                , bay(cast<g_type>(b_acc_y))
                , baz(cast<g_type>(b_acc_z))
                , bwx(cast<g_type>(b_ang_vel_x))
                , bwy(cast<g_type>(b_ang_vel_y))
                , bwz(cast<g_type>(b_ang_vel_z))
            {}
            Bias(const fltp08& b_acc_x, const fltp08& b_acc_y, const fltp08& b_acc_z,
                const fltp08& b_ang_vel_x, const fltp08& b_ang_vel_y, const fltp08& b_ang_vel_z)
                : bax(cast<g_type>(b_acc_x))
                , bay(cast<g_type>(b_acc_y))
                , baz(cast<g_type>(b_acc_z))
                , bwx(cast<g_type>(b_ang_vel_x))
                , bwy(cast<g_type>(b_ang_vel_y))
                , bwz(cast<g_type>(b_ang_vel_z))
            {}
            void CopyFrom(const Bias& b);
 
        public:
            g_type bax, bay, baz; 
            g_type bwx, bwy, bwz; 
            EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        };

        class Calib
        {
        public:
 
            Calib(const Sophus::SE3<g_type>& Tbc, const g_type& ng, const g_type& na, const g_type& ngw, const g_type& naw)
            {
                Set(Tbc, ng, na, ngw, naw);
            }
 
            Calib(const Calib& calib);
            Calib() { mbIsSet = false; }

            void Set(const Sophus::SE3<g_type>& sophTbc, const g_type& ng, const g_type& na, const g_type& ngw, const g_type& naw);
 
        public:
            Sophus::SE3<g_type> mTcb; 
            Sophus::SE3<g_type> mTbc; 
            Eigen::DiagonalMatrix<g_type, 6> Cov, CovWalk; 
            g_type gyro_random_walk = 0.0f; 
            g_type acc_random_walk = 0.0f; 
            bool mbIsSet = false; 
        };

        class IntegratedRotation
        {
        public:
            IntegratedRotation() {}
            IntegratedRotation(const Eigen::Vector3<g_type>& angVel, const Bias& imuBias, const g_type& time);
 
        public:
            g_type deltaT = 0.0f; 
            Eigen::Matrix3f deltaR; 
            Eigen::Matrix3f rightJ; 
            EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        };

        class Preintegrated
        {
        public:
            EIGEN_MAKE_ALIGNED_OPERATOR_NEW
            Preintegrated(const Bias& b_, const Calib& calib);
            Preintegrated(const Preintegrated& pImuPre);
            Preintegrated() {}
            ~Preintegrated() {}
            void CopyFrom(const Preintegrated& pImuPre);
            void Initialize(const Bias& b_);
            void IntegrateNewMeasurement(const Eigen::Vector3<g_type>& acceleration, const Eigen::Vector3<g_type>& angVel, const g_type dt);
            void Reintegrate();
            void MergePrevious(Preintegrated* pPrev);
            void SetNewBias(const Bias& bu_);
            IMU::Bias GetDeltaBias(const Bias& b_);
            Eigen::Matrix3<g_type> GetDeltaRotation(const Bias& b_);
            Eigen::Vector3<g_type> GetDeltaVelocity(const Bias& b_);
            Eigen::Vector3<g_type> GetDeltaPosition(const Bias& b_);
            Eigen::Matrix3<g_type> GetUpdatedDeltaRotation();
            Eigen::Vector3<g_type> GetUpdatedDeltaVelocity();
            Eigen::Vector3<g_type> GetUpdatedDeltaPosition();
            Eigen::Matrix3<g_type> GetOriginalDeltaRotation();
            Eigen::Vector3<g_type> GetOriginalDeltaVelocity();
            Eigen::Vector3<g_type> GetOriginalDeltaPosition();
            Eigen::Matrix<g_type, 6, 1> GetDeltaBias();
            Bias GetOriginalBias();
            Bias GetUpdatedBias();
        public:
            g_type dT = 0.0f; 
            Eigen::Matrix<g_type, 15, 15> calc; 
            Eigen::Matrix<g_type, 15, 15> Info; 
            Eigen::DiagonalMatrix<g_type, 6> Nga, NgaWalk; 
 
            Bias b; 
            Eigen::Matrix3<g_type> dR; 
            Eigen::Vector3<g_type> dV, dP; 
            Eigen::Matrix3<g_type> JRg, JVg, JVa, JPg, JPa; 
            Eigen::Vector3<g_type> avgA, avgW; 
 
 
        private:
            Bias bu; 
            Eigen::Matrix<g_type, 6, 1> db; 

            struct integrable
            {
                EIGEN_MAKE_ALIGNED_OPERATOR_NEW
                integrable() {}
                integrable(const Eigen::Vector3<g_type>& a_, const Eigen::Vector3<g_type>& w_, const g_type& t_) :a(a_), w(w_), t(t_) {}
                Eigen::Vector3<g_type> a, w; 
                g_type t = 0.0f; 
            };
 
            Buffer<integrable> mvMeasurements; 
 
            std::mutex mMutex; 
        };

        static Eigen::Matrix3<g_type> RightJacobianSO3(const g_type& x, const g_type& y, const g_type& z);
        static Eigen::Matrix3<g_type> RightJacobianSO3(const Eigen::Vector3<g_type>& v);
        static Eigen::Matrix3<g_type> InverseRightJacobianSO3(const g_type& x, const g_type& y, const g_type& z);
        static Eigen::Matrix3<g_type> InverseRightJacobianSO3(const Eigen::Vector3<g_type>& v);
        extern Eigen::Matrix3<g_type> NormalizeRotation(const Eigen::Matrix3<g_type>& R);
 
    };
}
#endif