The imu_initialization from dzunigan

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?

Acc Bias

Great work!
How do you calculate the acc bias? I do not konw how to reproduce the data provided in the paper.

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?

how to set the proper prior for the accelerometer bias?

thanks for your great work!
in issue 4, you said the prior of the accelerometer bias matters, so how to set the proper prior for the accelerometer bias when using different dataset, other than euror? line

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

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.

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.

dzunigan / imu_initialization Goto Github PK

imu_initialization's People

Contributors

Stargazers

Watchers

Forkers

imu_initialization's Issues

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

Acc Bias

Covariance in the code

how to set the proper prior for the accelerometer bias?

Reference to the paper

Imu_Init with Vins-Mono

proposed_noweight

Recommend Projects

React

Vue.js

Typescript

TensorFlow

Django

Laravel

D3

Recommend Topics

javascript

web

server

Machine learning

Visualization

Game

Recommend Org

Facebook

Microsoft

Google

Alibaba

D3

Tencent

Jobs