Implementation of "An Analytical Solution to the IMU Initialization Problem for Visual-Inertial Systems"

An Analytical Solution to the IMU Initialization Problem for Visual-Inertial Systems

Implementation of "An Analytical Solution to the IMU Initialization Problem for Visual-Inertial Systems"

Authors: David Zuñiga-Noël, Francisco-Angel Moreno and Javier Gonzalez-Jimenez

Dependencies (tested)

  • CMake (3.10.2):

    sudo apt install cmake
    
  • Boost (1.65.1):

    sudo apt install libboost-all-dev
    
  • Eigen3 (3.3.4):

    sudo apt install libeigen3-dev
    
  • Gflags (2.2.1):

    sudo apt install libgflags-dev
    
  • Glog (0.3.5)

    sudo apt install libgoogle-glog-dev
    
  • Ceres-solver (2.0.0)

    Install ceres-solver-2.0.0 following these instructions. Requires additionally libatlas-base-dev and libsuitesparse-dev.

Build

Make sure all dependencies are correctly installed. To build, just run the provided build.sh script:

git clone https://github.com/dzunigan/imu_initializaiton
bash build.sh

which should build the executables in the ./build directory.

Source structure

  • The analytical solution is implemented in function proposed_accelerometer() in include/methods.h

  • The non-linear constfunctions for iterative optimzation with ceres can be found in imu_ceres.h

  • The iterative alternative is implemented in function iterative() in include/methods.h

  • The IMU preintegration code (adapted from ORB_SLAM3) is implemented in: include/imu_preintegration.h src/imu_preintegration.cc

Comments
  • Imu_Init with Vins-Mono

    Imu_Init with Vins-Mono

    Hello, thanks for your contribution.

    I am trying to add the code to Vins-mono, but the gyro bias is not converged in my code.

    The gryo bias factor code as follows

    class GyroscopeBiasCostFunction : public ceres::SizedCostFunction<3, 3> {
        public:
            GyroscopeBiasCostFunction(std::shared_ptr<IntegrationBase> pIntj, const Eigen::Matrix3d &Ri, const Eigen::Matrix3d &Rj) :
                pIntj_(pIntj), Ri_(Ri), Rj_(Rj) 
            {
                Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver(pIntj_ -> covariance.block<3,3>(3,3));
                SqrtInformation_ = solver.operatorSqrt();
            }
    
            virtual ~GyroscopeBiasCostFunction() {}
    
            bool Evaluate(double const* const* parameters, double* residuals, double** jacabians) const override {
                Eigen::Map<const Eigen::Vector3d> bg(parameters[0]);
    
                Eigen::Matrix3d dq_dbg = pIntj_ -> jacobian.block<3, 3>(3, 12);
                Eigen::Vector3d dbg = bg - pIntj_ ->linearized_bg;
                Eigen::Quaterniond corrected_delta_q = pIntj_ -> delta_q * Utility::deltaQ(dq_dbg * dbg);
                // Eigen::Vector3d delta_bg = pIntj_ -> jacobian.block<3,3>(3,12) * (bg - pIntj_ -> linearized_bg);
                // Eigen::Matrix3d deltaR = pIntj_ -> delta_q.toRotationMatrix() * Utility::ExpSO3(delta_bg.x(), delta_bg.y(), delta_bg.z());
                const Eigen::Matrix3d eR = corrected_delta_q.toRotationMatrix().transpose() * Ri_.transpose() * Rj_;
                const Eigen::Vector3d err = Utility::LogSO3(eR);
    
                Eigen::Map<Eigen::Vector3d> e(residuals);
                e = err;
                e = SqrtInformation_ * e;
    
                if(jacabians != nullptr) {
                    if(jacabians[0] != nullptr) {
                    
                        const Eigen::Matrix3d invJr = Utility::InverseRightJacobianSO3(err[0], err[1], err[2]);
    
                        Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> J(jacabians[0]);
                        Eigen::Vector3d J_RbgMultipDbg = pIntj_ -> jacobian.block<3,3>(3,12) * dbg;
                        J = -invJr * eR.transpose() * Utility::RightJacobianSO3(J_RbgMultipDbg.x(), J_RbgMultipDbg.y(), J_RbgMultipDbg.z()) * pIntj_ -> jacobian.block<3,3>(3,12);
                        J = SqrtInformation_ * J;
                    }
                }
    
                return true;
            }
    
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
        private:
        std::shared_ptr<IntegrationBase> pIntj_;
        const Eigen::Matrix3d Ri_, Rj_;
        Eigen::Matrix3d SqrtInformation_;
    };
    

    Then my optimiation code as follows:

        Eigen::Vector3d bias_;
        bias_.setZero();
    
        ceres::Problem problem;
        map<double, ImageFrame>::iterator frame_i;
        map<double, ImageFrame>::iterator frame_j;
        for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++) {
            frame_j = next(frame_i);
            const Eigen::Matrix3d &Ri = frame_i->second.R;
            const Eigen::Matrix3d &Rj = frame_j->second.R;
            
            std::shared_ptr<IntegrationBase> p_int(new IntegrationBase(*(frame_j -> second.pre_integration)));
            ceres::CostFunction* cost_function = new GyroscopeBiasCostFunction(p_int, Ri, Rj);
            problem.AddResidualBlock(cost_function, nullptr, bias_.data());
        }
    
        TicToc time0;
        ceres::Solver::Options options;
        options.minimizer_progress_to_stdout = true;
        ceres::Solver::Summary summary;
    

    But the final output of bias is Zero. Then I printed the optimization process and found that the cost was very small before optimization. iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time 0 1.578785e-12 0.00e+00 1.54e-11 0.00e+00 0.00e+00 1.00e+04 0 3.79e-05 8.99e-05

    So I would like to ask you, what is wrong with the code. Thank you very much again.

  • Whether your method can deal with pure rotational motion or non-accelerated motions

    Whether your method can deal with pure rotational motion or non-accelerated motions

    Thank you for your excellent work!I would like to ask you whether the method you proposed is applicable to the VIO initialization process when the vehicle is moving at uniform speed or in a straight line?

  • proposed_noweight

    proposed_noweight

    Thanks for your work. When I compile this code, there is an error about "proposed_noweight()" in experiment01a . It seems that this function was not declared in this scope. So I don't know how to deal with.

  • Reference to the paper

    Reference to the paper

    This is a great implementation, but I cannot find the similar paper mentioned and I am assuming this IMU initialization is similar to one mentioned in ORB SLAM 3 where they are talking about Inertial only optimization and also some reference from Visual Inertial monocular SLAM with map reuse

  • Covariance in the code

    Covariance in the code

    Thanks for your work. I have some confusion about the covariance calculation in line 196 in include/methods.h, and I have not found the related formula in the paper. Is there any derivation about that? cov

A CUDA implementation of Lattice Boltzmann for fluid dynamics simulation
A CUDA implementation of Lattice Boltzmann for fluid dynamics simulation

Lattice Boltzmann simulation I am conscious of being only an individual struggling weakly against the stream of time. But it still remains in my power

Mar 1, 2022
The official implementation of our CVPR 2021 paper - Hybrid Rotation Averaging: A Fast and Robust Rotation Averaging Approach

Graph Optimizer This repo contains the official implementation of our CVPR 2021 paper - Hybrid Rotation Averaging: A Fast and Robust Rotation Averagin

Nov 13, 2022
Implementation for the "Surface Reconstruction from 3D Line Segments" paper.
Implementation for the

Surface Reconstruction from 3D Line Segments Surface reconstruction from 3d line segments. [Paper] [Supplementary Material] Langlois, P. A., Boulch, A

Nov 26, 2022
C++ Implementation of "An Equivariant Filter for Visual Inertial Odometry", ICRA 2021

EqF VIO (Equivariant Filter for Visual Inertial Odometry) This repository contains an implementation of an Equivariant Filter (EqF) for Visual Inertia

Nov 15, 2022
Unofficial third-party implementation of FFD (fast feature detector) published in IEEE TIP 2020.

fast_feature_detector Unofficial third-party implementation of FFD (fast feature detector) published in IEEE TIP 2020. Caution I have not got any perm

Feb 17, 2022
ResNet Implementation, Training, and Inference Using LibTorch C++ API

LibTorch C++ ResNet CIFAR Example Introduction ResNet implementation, training, and inference using LibTorch C++ API. Because there is no native imple

Oct 29, 2022
Swin Transformer C++ Implementation
Swin Transformer C++ Implementation

This is Swin Transformer C++ Implementation, inspired by swin-transformer-pytorch.

Nov 7, 2022
Implementation of EVO (RA-L 17)
Implementation of EVO (RA-L 17)

EVO: Event based Visual Odometry Credit, License, and Patent Citation This code implements the event-based visual odometry pipeline described in the p

Nov 24, 2022
An Efficient Implementation of Analytic Mesh Algorithm for 3D Iso-surface Extraction from Neural Networks
An Efficient Implementation of Analytic Mesh Algorithm for 3D Iso-surface Extraction from Neural Networks

AnalyticMesh Analytic Marching is an exact meshing solution from neural networks. Compared to standard methods, it completely avoids geometric and top

Nov 27, 2022
AlphaZero like implementation for Oware Abapa game

CGZero AlphaZero like implementation for Oware abapa game, in Codingame (https://www.codingame.com/multiplayer/bot-programming/oware-abapa) See https:

Jul 2, 2022
An Efficient Implementation of Analytic Mesh Algorithm for 3D Iso-surface Extraction from Neural Networks
An Efficient Implementation of Analytic Mesh Algorithm for 3D Iso-surface Extraction from Neural Networks

AnalyticMesh Analytic Marching is an exact meshing solution from neural networks. Compared to standard methods, it completely avoids geometric and top

Sep 8, 2022
The official implementation of the research paper "DAG Amendment for Inverse Control of Parametric Shapes"
The official implementation of the research paper

DAG Amendment for Inverse Control of Parametric Shapes This repository is the official Blender implementation of the paper "DAG Amendment for Inverse

Sep 21, 2022
Python and C++ implementation of "MarkerPose: Robust real-time planar target tracking for accurate stereo pose estimation". Accepted at LXCV Workshop @ CVPR 2021.
Python and C++ implementation of

MarkerPose: Robust Real-time Planar Target Tracking for Accurate Stereo Pose Estimation This is a PyTorch and LibTorch implementation of MarkerPose: a

Nov 18, 2022
An implementation of [Jimenez et al., 2016] Ground Truth Ambient Occlusion, MIT license
An implementation of [Jimenez et al., 2016] Ground Truth Ambient Occlusion, MIT license

XeGTAO Introduction XeGTAO is an open source, MIT licensed, DirectX/HLSL implementation of the Practical Realtime Strategies for Accurate Indirect Occ

Nov 25, 2022
Provide sample code of efficient operator implementation based on the Cambrian Machine Learning Unit (MLU) .

Cambricon CNNL-Example CNNL-Example 提供基于寒武纪机器学习单元(Machine Learning Unit,MLU)开发高性能算子、C 接口封装的示例代码。 依赖条件 操作系统: 目前只支持 Ubuntu 16.04 x86_64 寒武纪 MLU SDK: 编译和

Mar 7, 2022
A c++ implementation of yolov5 and deepsort

A C++ implementation of Yolov5 and Deepsort in Jetson Xavier nx and Jetson nano This repository uses yolov5 and deepsort to follow humna heads which c

Nov 19, 2022
An implementation on Fast Ground Segmentation for 3D LiDAR Point Cloud Based on Jump-Convolution-Process.
An implementation on Fast Ground Segmentation for 3D LiDAR Point Cloud Based on Jump-Convolution-Process.

An implementation on "Shen Z, Liang H, Lin L, Wang Z, Huang W, Yu J. Fast Ground Segmentation for 3D LiDAR Point Cloud Based on Jump-Convolution-Process. Remote Sensing. 2021; 13(16):3239. https://doi.org/10.3390/rs13163239"

Nov 18, 2022
An inofficial PyTorch implementation of PREDATOR based on KPConv.

PREDATOR: Registration of 3D Point Clouds with Low Overlap An inofficial PyTorch implementation of PREDATOR based on KPConv. The code has been tested

Aug 3, 2022