vio/GPU_version/vio/gtsam/imu_integration.cpp

//
// Created by gfuse on 4/9/21.
//
//#define NDEBUG
#include "imu_integration.h"
#include <memory>
#include <utility>
#include "vio/feature.h"
#include "vio/point.h"
#include "vio/svoVisionFactor.h"
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/geometry/Pose3.h>
#include <vio/params_helper.h>
#include <vio/config.h>
#include <vio/for_it.hpp>
#define debug 0
Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){
    graphPtr=std::make_shared<gtsam::NonlinearFactorGraph>();
    valuesPtr=std::make_shared<gtsam::Values>();
    parameterPtr = boost::shared_ptr<gtsam::PreintegrationCombinedParams>(new gtsam::PreintegrationCombinedParams(gtsam::Vector3(0.0,0.0,0.0)));
    parameterPtr->accelerometerCovariance=gtsam::I_3x3 * svo::Config::ACC_Noise(); // acc white noise in continuous
    parameterPtr->gyroscopeCovariance=gtsam::I_3x3 * svo::Config::GYO_Noise();// gyro white noise in continuous
    parameterPtr->integrationCovariance=gtsam::I_3x3 * svo::Config::IUC(); // integration uncertainty continuous
    parameterPtr->biasAccCovariance = gtsam::I_3x3 * svo::Config::ABC();   // acc bias in continuous
    parameterPtr->biasOmegaCovariance = gtsam::I_3x3 * svo::Config::GBC();  // gyro bias in continuous
    parameterPtr->biasAccOmegaInt = gtsam::I_6x6 * svo::Config::EBP();  // error in the bias used for preintegration
    imu_biasPtr=std::make_shared<gtsam::imuBias::ConstantBias>(gtsam::Vector3(1e-8,1e-8,1e-8),gtsam::Vector3(1e-8,1e-8,1e-8));
    preintegratedPtr = std::make_shared<gtsam::PreintegratedCombinedMeasurements>(parameterPtr,*imu_biasPtr);
    ProjectNoisePtr = gtsam::noiseModel::Isotropic::Sigma(2, svo::Config::NRP()); // the noise used for projection
    optimizerParamPtr=std::make_shared<gtsam::ISAM2Params>();
    optimizerParamPtr->relinearizeThreshold = 0.1;
    optimizerParamPtr->relinearizeSkip = 1;
    optimizerParamPtr->evaluateNonlinearError=true;
    optimizerParamPtr->enableDetailedResults=true;
    if(debug)logtime = fopen((std::string(getenv("HOME"))+"/imu_preIntegrate.txt").c_str(), "w+");
    assert(preintegratedPtr);
}
Imu_Integration::~Imu_Integration(){

}
bool Imu_Integration::update(double* imu){
    if(syn)return false;
    if(imu_factor_id<1)return false;
    auto in=std::chrono::steady_clock::now();
    if(std::chrono::duration<double>(in-t_1).count()>20.0){
        t_1=in;
        preintegratedPtr->integrateMeasurement(Eigen::Vector3d(-1.0*(imu[0]-svo::Config::X_off()),0.0,-1.0*(imu[1]-svo::Config::Y_off())),Eigen::Vector3d(0.0,imu[2]-svo::Config::Teta_off(),0.0),0.0005);
    }else{
        preintegratedPtr->integrateMeasurement(Eigen::Vector3d(-1.0*(imu[0]-svo::Config::X_off()),0.0,-1.0*(imu[1]-svo::Config::Y_off())),Eigen::Vector3d(0.0,imu[2]-svo::Config::Teta_off(),0.0),std::chrono::duration<double>(in-t_1).count());
        t_1=in;
        ++imu_n;
    }
    return true;

}
bool Imu_Integration::reset(gtsam::LevenbergMarquardtOptimizer& optimizer,boost::shared_ptr<svo::Frame>& new_frame){
    gtsam::Values result = optimizer.optimize();
    statePtr=std::make_shared<gtsam::NavState>(result.at<gtsam::Pose3>(P(imu_factor_id)), result.at<gtsam::Vector3>(V(imu_factor_id)));
    imu_biasPtr=std::make_shared<gtsam::imuBias::ConstantBias>(result.at<gtsam::imuBias::ConstantBias>(B(imu_factor_id)));
    preintegratedPtr->resetIntegrationAndSetBias(*imu_biasPtr);
    ++imu_factor_id;
    syn=false;
    gtsam::Marginals marginals(*graphPtr,result);
    if(debug)fprintf(logtime, "%d,%f,%f,%f,%f,\n",
                     imu_factor_id-1,
                     statePtr->pose().x(),
                     statePtr->pose().y(),
                     statePtr->pose().z(),
                     statePtr->pose().rotation().pitch());
    new_frame->T_f_w_=svo::SE2_5(statePtr->pose().x(),statePtr->pose().z(),statePtr->pose().rotation().pitch());
    new_frame->Cov_=marginals.marginalCovariance(P(imu_factor_id-1));
    graphPtr->resize(0);
    valuesPtr->clear();
    valuesPtr->insert(P(imu_factor_id-1),gtsam::Pose3(new_frame->T_f_w_.T->matrix()));
    valuesPtr->insert(V(imu_factor_id-1),statePtr->v());
    valuesPtr->insert(B(imu_factor_id-1),*imu_biasPtr);
    graphPtr->addPrior(P(imu_factor_id-1),gtsam::Pose3(new_frame->T_f_w_.T->matrix()),marginals.marginalCovariance(P(imu_factor_id-1)));
    graphPtr->addPrior(V(imu_factor_id-1),statePtr->v(),marginals.marginalCovariance(V(imu_factor_id-1)));
    graphPtr->addPrior(B(imu_factor_id-1),*imu_biasPtr,marginals.marginalCovariance(B(imu_factor_id-1)));
    return true;
}
bool Imu_Integration::preintegrate_predict(svo::SE2_5& frame) {
    if(imu_n<1){
        syn= false;
        return false;
    }else{
        syn=true;
    }
    Integ_predict=preintegratedPtr->predict(*statePtr,*imu_biasPtr);
    frame =svo::SE2_5(Integ_predict.pose().x(),Integ_predict.pose().z(),Integ_predict.pose().rotation().pitch());
    return true;
}
void Imu_Integration::reset() {
    preintegratedPtr->resetIntegrationAndSetBias(*imu_biasPtr);
    imu_n=0;
    syn=false;
}
bool Imu_Integration::predict(boost::shared_ptr<svo::Frame>& new_frame,
                              boost::shared_ptr<svo::Frame>& last_frame,
                              const double cell_match){
    valuesPtr->insert(P(imu_factor_id), Integ_predict.pose());
    valuesPtr->insert(V(imu_factor_id), Integ_predict.v());
    valuesPtr->insert(B(imu_factor_id), *imu_biasPtr);
    gtsam::CombinedImuFactor imu_factor(P(imu_factor_id - 1), V(imu_factor_id - 1), P(imu_factor_id), V(imu_factor_id), B(imu_factor_id - 1), B(imu_factor_id),
                                        *preintegratedPtr);
    graphPtr->emplace_shared<gtsam::CombinedImuFactor>(imu_factor);
    imu_n=0;
    graphPtr->emplace_shared<svo::VisionFactor>(gtsam::Pose3(new_frame->T_f_w_.T->matrix()),
                                                gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(3) << 1.0, 1.0, 1.0).finished()/(cell_match+1)),
                                                P(imu_factor_id));
    gtsam::LevenbergMarquardtParams params0;
    params0.verbosityLM = gtsam::LevenbergMarquardtParams::SILENT;
    params0.setlambdaFactor(10.0);
    params0.setlambdaInitial(100);
    params0.setlambdaUpperBound(1e10);
    gtsam::LevenbergMarquardtOptimizer optimizer(*graphPtr, *valuesPtr,params0);
    reset(optimizer,new_frame);
    return true;
}
bool Imu_Integration::init(boost::shared_ptr<svo::Frame>& new_frame){
    statePtr=std::make_shared<gtsam::NavState>(gtsam::Pose3(new_frame->T_f_w_.T->matrix()),
                                               gtsam::Vector3(1e-9,1e-9,1e-9));
    valuesPtr->insert(P(0),statePtr->pose());
    valuesPtr->insert(V(0),statePtr->v());
    valuesPtr->insert(B(0),*imu_biasPtr);
    graphPtr->addPrior(P(0),statePtr->pose(),gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-5, 1e-15, 1e-5, 1e-15, 1e-8, 1e-15).finished()));
    graphPtr->addPrior(V(0),statePtr->v(),gtsam::noiseModel::Isotropic::Sigma(3, 1e-4));
    graphPtr->addPrior(B(0),*imu_biasPtr,gtsam::noiseModel::Isotropic::Sigma(6, 1e-2));
    ++imu_factor_id;
    return true;
}