IMU預(yù)積分功能數(shù)據(jù)初始化代碼解讀

代碼解讀

int main(int argc, char** argv){    ros::init(argc, argv, "roboat_loam");    IMUPreintegration ImuP;//IMUPreintegration 類的實(shí)例    TransformFusion TF;//TransformFusion 類的實(shí)例    ROS_INFO("?33[1;32m---- > IMU Preintegration Started.?33[0m");//打印消息    ros::MultiThreadedSpinner spinner(4);//開四個(gè)線程 通過并發(fā)的方式使得速度得到提升    spinner.spin();//程序執(zhí)行到這個(gè)地方 則等待 topic 回調(diào)函數(shù)執(zhí)行    return 0;}

main函數(shù)部分很簡(jiǎn)潔，功能主要完成部分都在定義的兩個(gè)類中進(jìn)行。

在main函數(shù)中進(jìn)行

• 節(jié)點(diǎn)初始化
• IMUPreintegration 類的實(shí)例
• TransformFusion 類的實(shí)例
• 打印消息
• 開四個(gè)線程 通過并發(fā)的方式使得速度得到提升
• 等待 topic 回調(diào)函數(shù)執(zhí)行
• 之后則看 IMUPreintegration 這個(gè)類，先看構(gòu)造函數(shù)部分

在里面首先進(jìn)行了 訂閱imu信息

subImu      = nh.subscribe< sensor_msgs::Imu >  (imuTopic,2000, &IMUPreintegration::imuHandler,this, ros::TransportHints().tcpNoDelay());

imuTopic 為 imu_correct, imu原始數(shù)據(jù)，這個(gè)imuTopic是從參數(shù)服務(wù)器讀取的，具體的配置在prams.yaml中

如果你的imu的topic和默認(rèn)的不一致則需要修改

然后可以看其具體的回調(diào)函數(shù) imuHandler

void imuHandler(const sensor_msgs::Imu::ConstPtr& imu_raw)    {        std::lock_guard< std::mutex > lock(mtx);        //首先把imu的狀態(tài)做一個(gè)簡(jiǎn)單的轉(zhuǎn)換        sensor_msgs::Imu thisImu = imuConverter(*imu_raw);        // 注意這里有兩個(gè)imu隊(duì)列，作用不相同，一個(gè)用來執(zhí)行預(yù)積分和位姿變換的優(yōu)化，一個(gè)用來更新最新imu狀態(tài)          imuQueOpt.push_back(thisImu);        imuQueImu.push_back(thisImu);        // 如果沒有發(fā)生過優(yōu)化 則 return        if (doneFirstOpt == false)            return;

回調(diào)函數(shù)先把imu的狀態(tài)做一個(gè)簡(jiǎn)單的轉(zhuǎn)換，轉(zhuǎn)到lidar坐標(biāo)系 下

將轉(zhuǎn)換后的imu數(shù)據(jù)存入兩個(gè)隊(duì)列中，注意這里有兩個(gè)imu隊(duì)列，作用不相同，一個(gè)用來執(zhí)行預(yù)積分和位姿變換的優(yōu)化，一個(gè)用來更新最新imu狀態(tài)

如果沒有發(fā)生過優(yōu)化 則 retur，doneFirstOpt這個(gè)標(biāo)志位，在接受到幀間里程計(jì)信息后，則至為true，imuConverter函數(shù)在utility.h文件中

sensor_msgs::Imu imuConverter(const sensor_msgs::Imu& imu_in)    {        sensor_msgs::Imu imu_out = imu_in;        // rotate acceleration  //進(jìn)行加速度坐標(biāo)旋轉(zhuǎn)        Eigen::Vector3d acc(imu_in.linear_acceleration.x, imu_in.linear_acceleration.y, imu_in.linear_acceleration.z);        acc = extRot * acc;        imu_out.linear_acceleration.x = acc.x();        imu_out.linear_acceleration.y = acc.y();        imu_out.linear_acceleration.z = acc.z();        // rotate gyroscope  // 進(jìn)行陀螺儀坐標(biāo)旋轉(zhuǎn)        Eigen::Vector3d gyr(imu_in.angular_velocity.x, imu_in.angular_velocity.y, imu_in.angular_velocity.z);        gyr = extRot * gyr;        imu_out.angular_velocity.x = gyr.x();        imu_out.angular_velocity.y = gyr.y();        imu_out.angular_velocity.z = gyr.z();        // rotate roll pitch yaw // 進(jìn)行姿態(tài)角坐標(biāo)旋轉(zhuǎn)        Eigen::Quaterniond q_from(imu_in.orientation.w, imu_in.orientation.x, imu_in.orientation.y, imu_in.orientation.z);        Eigen::Quaterniond q_final = q_from * extQRPY;        imu_out.orientation.x = q_final.x();        imu_out.orientation.y = q_final.y();        imu_out.orientation.z = q_final.z();        imu_out.orientation.w = q_final.w();        //檢測(cè)姿態(tài)數(shù)據(jù)是否正常        if (sqrt(q_final.x()*q_final.x() + q_final.y()*q_final.y() + q_final.z()*q_final.z() + q_final.w()*q_final.w()) < 0.1)        {            ROS_ERROR("Invalid quaternion, please use a 9-axis IMU!");            ros::shutdown();        }        return imu_out;//返回變換后的imu數(shù)據(jù)    }};

進(jìn)行加速度坐標(biāo)旋轉(zhuǎn)、進(jìn)行陀螺儀坐標(biāo)旋轉(zhuǎn)、進(jìn)行姿態(tài)角坐標(biāo)旋轉(zhuǎn)、檢測(cè)姿態(tài)數(shù)據(jù)是否正常、返回變換后的imu數(shù)據(jù)

在進(jìn)行加速度和陀螺儀變換的時(shí)候，使用的是extRot，該參數(shù)的根源來源于prams.yaml中

進(jìn)行姿態(tài)角坐標(biāo)旋轉(zhuǎn)，使用的是extQRPY，該參數(shù)的根源來源于prams.yaml中

所有終于明白為什么在配置文件中有兩個(gè)外參了！

imuHandler這個(gè)回調(diào)函數(shù)，先看到這部分，后面的之后再看，需要回到上面的IMUPreintegration的構(gòu)造函數(shù)，看訂閱到幀間里程計(jì)信息做了哪些事情。

subOdometry = nh.subscribe< nav_msgs::Odometry >("lio_sam/mapping/odometry_incremental", 5,    &IMUPreintegration::odometryHandler, this, ros::TransportHints().tcpNoDelay());

訂閱雷達(dá)里程計(jì)信息，lio_sam/mapping/odometry_incremental 是mapOptmization發(fā)出的，odometryHandler回調(diào)函數(shù)，走起

double currentCorrectionTime = ROS_TIME(odomMsg);

通過ROS_TIME函數(shù)把消息中的時(shí)間戳取了出來

if (imuQueOpt.empty())            return;

保證imu隊(duì)列中有數(shù)據(jù)

float p_x = odomMsg- >pose.pose.position.x;        float p_y = odomMsg- >pose.pose.position.y;        float p_z = odomMsg- >pose.pose.position.z;        float r_x = odomMsg- >pose.pose.orientation.x;        float r_y = odomMsg- >pose.pose.orientation.y;        float r_z = odomMsg- >pose.pose.orientation.z;        float r_w = odomMsg- >pose.pose.orientation.w;

通過里程計(jì)話題獲得位置信息 四元數(shù) 獲得雷達(dá)里程計(jì)位姿

bool degenerate = (int)odomMsg- >pose.covariance[0] == 1 ? true : false;

該位姿是否出現(xiàn)退化 pose.covariance[0] 為1 則 雷達(dá)里程計(jì)有退化風(fēng)險(xiǎn)，該幀位姿精度有一定程序下降

gtsam::Pose3 lidarPose = gtsam::Pose3(gtsam::Rot3::Quaternion(r_w, r_x, r_y, r_z), gtsam::Point3(p_x, p_y, p_z));

把位姿轉(zhuǎn)成 gtsam的格式，進(jìn)入系統(tǒng)的初始化，下面部分僅執(zhí)行一次

resetOptimization();

在函數(shù)內(nèi)部 初始化gtsam的一些量

while (!imuQueOpt.empty())            {                if (ROS_TIME(&imuQueOpt.front()) < currentCorrectionTime - delta_t)                {                    lastImuT_opt = ROS_TIME(&imuQueOpt.front());                    imuQueOpt.pop_front();                }                else                    break;            }

將這個(gè)雷達(dá)里程計(jì)之前的imu信息全部扔掉，整個(gè)LIO-SAM中作者對(duì)時(shí)間同步這塊的思想都是這樣的，保證imu與odometry消息時(shí)間同步 因?yàn)閕mu是高頻數(shù)據(jù)所以這是必要的

prevPose_ = lidarPose.compose(lidar2Imu);

將lidar的位姿移到imu坐標(biāo)系下，lidar2Imu 是lidar到imu的外參，compose是gtsam的一個(gè)功能函數(shù)，VIO和LIO的框架都在在IMU坐標(biāo)系下進(jìn)行的

gtsam::PriorFactor< gtsam::Pose3 > priorPose(X(0), prevPose_, priorPoseNoise);            graphFactors.add(priorPose);

設(shè)置其初始位姿和置信度，約束加入到因子中

• gtsam::PriorFactor 模塊涉及到的變量結(jié)點(diǎn)
• gtsam::Pose3 表示六自由度位姿
• gtsam::Vector3 表示三自由度速度
• gtsam::imuBias::ConstantBias 表示IMU零偏

以上也是預(yù)積分模型中涉及到的三種狀態(tài)變量

gtsam::PriorFactor 為先驗(yàn)因子，表示對(duì)某個(gè)狀態(tài)量T的一個(gè)先驗(yàn)估計(jì)，約束某個(gè)狀態(tài)變量的狀態(tài)不會(huì)離該先驗(yàn)值過遠(yuǎn)。

其中的X(0)的，初始定義如下。事先的符號(hào)

priorPoseNoise 是先驗(yàn)位姿的噪聲，該值為

priorPoseNoise  = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) < < 1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2).finished()); // rad,rad,rad,m, m, m

初始 位姿 置信度 設(shè)置 比較高 后面構(gòu)成協(xié)方差矩陣 值越小 表示 置信度越高

prevVel_ = gtsam::Vector3(0, 0, 0);            gtsam::PriorFactor< gtsam::Vector3 > priorVel(V(0), prevVel_, priorVelNoise);               graphFactors.add(priorVel);

和上面位姿基本一樣初始化速度，這里直接賦 0 了，將速度約束加到因子圖中，其中priorVelNoise 速度的噪聲是

priorVelNoise   = gtsam::noiseModel::Isotropic::Sigma(3, 1e4); // m/s

初始化速度 置信度 設(shè)置 差些 因?yàn)樗俣纫婚_始設(shè)置的是0，不知道是多少

prevBias_ = gtsam::imuBias::ConstantBias();            gtsam::PriorFactor< gtsam::imuBias::ConstantBias > priorBias(B(0), prevBias_, priorBiasNoise);                  graphFactors.add(priorBias);

初始化IMU 零偏 ，將零偏約束加到因子圖中，gtsam::imuBias::ConstantBias()是gtsam做好的一個(gè)imu零偏，其中都是0，所以對(duì)應(yīng)bias的噪聲置信度也要設(shè)置的高些

priorBiasNoise  = gtsam::noiseModel::Isotropic::Sigma(6, 1e-3); // 1e-2 ~ 1e-3 seems to be good

以上把約束加入完畢，下面就開始添加狀態(tài)量

graphValues.insert(X(0), prevPose_);            graphValues.insert(V(0), prevVel_);            graphValues.insert(B(0), prevBias_);

給各個(gè)狀態(tài)量賦成初始值

optimizer.update(graphFactors, graphValues);

約束和狀態(tài)量更新 進(jìn)isam優(yōu)化器

graphFactors.resize(0);            graphValues.clear();

進(jìn)優(yōu)化器之后 保存約束和狀態(tài)量的變量就清零

imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);            imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);

預(yù)積分的接口，使用初始零偏進(jìn)行初始化 之前imu有兩個(gè)隊(duì)列，每個(gè)隊(duì)列對(duì)應(yīng)預(yù)積分處理器

key = 1;
            systemInitialized = true;//系統(tǒng)初始化完成
            return;

系統(tǒng)初始化完成