Imu_initialization - 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

Draft: arXiv:2103.03389

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

Implementation using C in programming lab class when being Teacher Assistant.

C Programming Lab Implementation using C in programming lab class when being Teacher Assistant. The problems are here Problems and Description Week2 w

Aug 16, 2022
This is a simple UNITEST to test the implementation of the the various container types of the C++ standard template library

ft_container UNITest. This is a simple UNITEST to test the implementation of the the various container types of the C++ standard template library that

Sep 17, 2022
Implementation of kcp protocol based on c++11
Implementation of kcp protocol based on c++11

?? kcp-cpp A C++11 header-only kcp library,It has been heavily optimized to support native heartbeat packets and multithreading There are a lot of ins

Jul 4, 2022
Jinja2 C++ (and for C++) almost full-conformance template engine implementation

Jinja2С++ C++ implementation of the Jinja2 Python template engine. This library brings support of powerful Jinja2 template features into the C++ world

Sep 17, 2022
wideint is a C++ implementation of wide exact-width integer types.

wideint - wide exact-width integer types Copyright (c) 2022 Joergen Ibsen About wideint is a C++ implementation of wide exact-width integer types. #in

Jan 27, 2022
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 Initializati

Aug 31, 2022
In DFS-BFS Implementation In One Program Using Switch Case I am Using an Simple And Efficient Code of DFS-BFS Implementation.
In DFS-BFS Implementation In One Program Using Switch Case I am Using an Simple And Efficient Code of DFS-BFS Implementation.

DFS-BFS Implementation-In-One-Program-Using-Switch-Case-in-C Keywords : Depth First Search(DFS), Breadth First Search(BFS) In Depth First Search(DFS),

Nov 17, 2021
EASTL stands for Electronic Arts Standard Template Library. It is an extensive and robust implementation that has an emphasis on high performance.

EA Standard Template Library EASTL stands for Electronic Arts Standard Template Library. It is a C++ template library of containers, algorithms, and i

Sep 22, 2022
An Open Source Implementation of the Actor Model in C++

CAF: C++ Actor Framework CAF is an open source implementation of the actor model for C++ featuring lightweight & fast actor implementations, pattern m

Sep 15, 2022
an efficient feature complete C++ bittorrent implementation
an efficient feature complete C++ bittorrent implementation

libtorrent is an open source C++ library implementing the BitTorrent protocol, along with most popular extensions, making it suitable for real world d

Sep 19, 2022
Sep 18, 2022
An implementation of Actor, Publish-Subscribe, and CSP models in one rather small C++ framework. With performance, quality, and stability proved by years in the production.
An implementation of Actor, Publish-Subscribe, and CSP models in one rather small C++ framework. With performance, quality, and stability proved by years in the production.

What is SObjectizer? What distinguishes SObjectizer? SObjectizer is not like TBB, taskflow or HPX Show me the code! HelloWorld example Ping-Pong examp

Sep 14, 2022
C++ implementation of a fast hash map and hash set using hopscotch hashing

A C++ implementation of a fast hash map and hash set using hopscotch hashing The hopscotch-map library is a C++ implementation of a fast hash map and

Sep 13, 2022
C++ implementation of a fast hash map and hash set using robin hood hashing

A C++ implementation of a fast hash map and hash set using robin hood hashing The robin-map library is a C++ implementation of a fast hash map and has

Sep 21, 2022
s2n : an implementation of the TLS/SSL protocols
s2n : an implementation of the TLS/SSL protocols

s2n is a C99 implementation of the TLS/SSL protocols that is designed to be simple, small, fast, and with security as a priority. It is released and l

Sep 15, 2022
A simple C++ 03/11/etc timer class for ~microsecond-precision cross-platform benchmarking. The implementation is as limited and as simple as possible to create the lowest amount of overhead.

plf_nanotimer A simple C++ 03/11/etc timer class for ~microsecond-precision cross-platform benchmarking. The implementation is as limited and simple a

Sep 18, 2022
C++ implementation of the Google logging module

Google Logging Library The Google Logging Library (glog) implements application-level logging. The library provides logging APIs based on C++-style st

Sep 25, 2022
jemalloc websitejemalloc - General purpose malloc(3) implementation that emphasizes fragmentation avoidance and scalable concurrency support. [BSD] website

jemalloc is a general purpose malloc(3) implementation that emphasizes fragmentation avoidance and scalable concurrency support. jemalloc first came

Sep 21, 2022
Single C file TLS 1.2/1.3 implementation, using tomcrypt as crypto library

TLSe Single C file TLS 1.3, 1.2, 1.1 and 1.0(without the weak ciphers) implementation, using libtomcrypt as crypto library. It also supports DTLS 1.2

Sep 21, 2022