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

Overview

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

Issues
  • 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.

    opened by NEU-LC 2
  • 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?

    opened by wangyuanbiubiubiu 1
  • 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.

    opened by Qiu0336 1
  • 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

    opened by ujasmandavia 1
  • 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

    opened by HT-MALS 0
Owner
David Zuniga-Noel
David Zuniga-Noel
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

Tsuen Hsueh 2 Dec 4, 2021
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

Mohamed AMOUSSAOUI 36 May 20, 2022
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

 KH 17 Apr 11, 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

Jinja2C++ project 353 Jun 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

Jørgen Ibsen 3 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

David Zuniga-Noel 68 Apr 29, 2022
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),

Rudra_deep 1 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

Electronic Arts 6.5k Jun 29, 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

CAF: C++ Actor Framework 2.8k Jul 1, 2022
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

Arvid Norberg 4k Jun 24, 2022
Concurrency Kit 2.1k Jun 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.

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

Stiffstream 289 Jun 29, 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

Thibaut Goetghebuer-Planchon 551 Jun 17, 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

Thibaut Goetghebuer-Planchon 771 Jun 24, 2022
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

Amazon Web Services 4.1k Jul 1, 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

Matt Bentley 88 Jun 20, 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

Google 5.4k Jul 1, 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

jemalloc memory allocator 7.1k Jun 27, 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

Eduard Suica 442 Jun 22, 2022
MessagePack implementation for C and C++ / msgpack.org[C/C++]

msgpack for C/C++ It's like JSON but smaller and faster. Overview MessagePack is an efficient binary serialization format, which lets you exchange dat

MessagePack 2.5k Jun 27, 2022
Protocol Buffers implementation in C

Overview This is protobuf-c, a C implementation of the Google Protocol Buffers data serialization format. It includes libprotobuf-c, a pure C library

null 2.1k Jun 28, 2022
a small protobuf implementation in C

μpb - a small protobuf implementation in C Platform Build Status macOS ubuntu μpb (often written 'upb') is a small protobuf implementation written in

Protocol Buffers 1.3k Jun 21, 2022
Open h.265 video codec implementation.

libde265 - open h.265 codec implementation libde265 is an open source implementation of the h.265 video codec. It is written from scratch and has a pl

struktur AG 1.3k Jun 24, 2022
MicroPython - a lean and efficient Python implementation for microcontrollers and constrained systems

The MicroPython project This is the MicroPython project, which aims to put an implementation of Python 3.x on microcontrollers and small embedded syst

MicroPython 14.5k Jun 30, 2022
Implementation of python itertools and builtin iteration functions for C++17

CPPItertools Range-based for loop add-ons inspired by the Python builtins and itertools library. Like itertools and the Python3 builtins, this library

Ryan Haining 1.2k Jun 23, 2022
Minimal A* implementation in C. No dynamic memory allocation.

Micro A Star Path Finder This is a minimal A* path finder implementation in C, without any dynamic memory allocation. Usage The maximum size of the ma

Felipe da Silva 76 Jun 11, 2022
Minimal A* implementation in C. No dynamic memory allocation.

Micro A Star Path Finder This is a minimal A* path finder implementation in C, without any dynamic memory allocation. Usage The maximum size of the ma

Felipe da Silva 76 Jun 11, 2022
An implementation of the MessagePack serialization format in C / msgpack.org[C]

CMP CMP is a C implementation of the MessagePack serialization format. It currently implements version 5 of the MessagePack Spec. CMP's goal is to be

Charlie Gunyon 280 Jun 21, 2022