ORB-SLAM3如何加入GPS和Wheel轮速约束

本文主要是介绍ORB-SLAM3如何加入GPS和Wheel轮速约束，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

0. 简介

对于ORB-SLAM3而言。如何将代码融入Wheel和GPS是一个挺有意思的事情。通过GPS和Wheel可以非常有效的约束视觉里程计结果。Wheel这块主要就是将速度等信息融合到前端中，类似IMU和视觉帧间的关系。而GPS由于频率不是很高，所以基本是用于全局修正的作用。这部分我们经常使用松耦合的形式，当然也有工作去做了紧耦合相关的工作。

1. Wheel特征添加

这一部分主要的其实就是将wheel的速度特征传入到imu中，以约束两者之间的位移信息。从而提供针对IMU的约束，并可以作为边约束约束G2O的边

1.1 ImuType.h

这一步主要是得是将轮速信息融合到IMU内部去做联合优化，这里面主要用于处理传感器数据融合和运动状态估计。

Point(const cv::Point3f Acc, const cv::Point3f Gyro, const double &timestamp, const double& encoder_v):a(Acc.x,Acc.y,Acc.z), w(Gyro.x,Gyro.y,Gyro.z), t(timestamp), encoder_v(encoder_v){}
.........
void IntegrateNewMeasurement(const Eigen::Vector3f &acceleration, const Eigen::Vector3f &angVel, const float &dt,const double &encoder_v );
.........Eigen::DiagonalMatrix<float,6> Nga, NgaWalk;Eigen::DiagonalMatrix<float,9> Nga_en, NgaWalk_en;// Values for the original bias (when integration was computed)Bias b;Eigen::Matrix3f dR;Eigen::Vector3f dV, dP;Eigen::Matrix3f Rbo = Eigen::Matrix3f::Identity();   //encoder 和imu的旋转Eigen::Vector3d Tbo = {-0.9810000061988831,0,0};   //encoder 和imu的旋转Eigen::Matrix3f JRg, JVg, JVa, JPg, JPa;Eigen::Vector3f avgA, avgW;Eigen::Vector3f encoder_velocity ;Eigen::Matrix<float,18,18> jacobian_enc;Eigen::Matrix<float,21,21> covariance_enc;

1.2 ImuType.cc

下面IntegrateNewMeasurement 函数中，该系统整合了惯性测量（来自加速度计和陀螺仪）和可选的编码器读数。此函数的主要目的是根据这些测量数据更新内部状态，以跟踪位置、速度和旋转的变化。

/** * @brief 初始化预积分* @param b_ 偏置*/
void Preintegrated::Initialize(const Bias &b_)
{dR.setIdentity();dV.setZero();dP.setZero();JRg.setZero();JVg.setZero();JVa.setZero();JPg.setZero();JPa.setZero();C.setZero();Info.setZero();db.setZero();encoder_velocity.setZero();jacobian_enc.setZero();covariance_enc.setZero();b = b_;bu = b_;  // 更新后的偏置avgA.setZero();  // 平均加速度avgW.setZero();  // 平均角速度dT = 0.0f;mvMeasurements.clear();  // 存放imu数据及dt
}/** * @brief 根据新的偏置重新积分mvMeasurements里的数据 Optimizer::InertialOptimization调用*/ 
void Preintegrated::Reintegrate()
{std::unique_lock<std::mutex> lock(mMutex);const std::vector<integrable> aux = mvMeasurements;//重新进行预积分Initialize(bu);bool buseencoder;buseencoder = true;if(buseencoder){for (size_t i = 0; i < aux.size(); i++)IntegrateNewMeasurement(aux[i].a, aux[i].w, aux[i].t,aux[i].enc);}else{for (size_t i = 0; i < aux.size(); i++)IntegrateNewMeasurement(aux[i].a, aux[i].w, aux[i].t);}
}void Preintegrated::IntegrateNewMeasurement(const Eigen::Vector3f &acceleration, const Eigen::Vector3f &angVel, const float &dt,const double &encoder_v){// 保存imu数据，利用中值积分的结果构造一个预积分类保存在mvMeasurements中mvMeasurements.push_back(integrable(acceleration, angVel, dt,encoder_v));// Position is updated firstly, as it depends on previously computed velocity and rotation.// Velocity is updated secondly, as it depends on previously computed rotation.// Rotation is the last to be updated.// Matrices to compute covariance// Step 1.构造协方差矩阵// 噪声矩阵的传递矩阵，这部分用于计算i到j-1历史噪声或者协方差Eigen::Matrix<float, 9, 9> A;A.setIdentity();// 噪声矩阵的传递矩阵，这部分用于计算j-1新的噪声或协方差，这两个矩阵里面的数都是当前时刻的，计算主要是为了下一时刻使用Eigen::Matrix<float, 9, 6> B;B.setZero();// 考虑偏置后的加速度、角速度Eigen::Vector3f acc, accW;acc << acceleration(0) - b.bax, acceleration(1) - b.bay, acceleration(2) - b.baz;accW << angVel(0) - b.bwx, angVel(1) - b.bwy, angVel(2) - b.bwz;// 记录平均加速度和角速度avgA = (dT * avgA + dR * acc * dt) / (dT + dt);avgW = (dT * avgW + accW * dt) / (dT + dt);// Update delta position dP and velocity dV (rely on no-updated delta rotation)// 根据没有更新的dR来更新dP与dV  eq.(38)dP = dP + dV * dt + 0.5f * dR * acc * dt * dt;dV = dV + dR * acc * dt;Eigen::Vector3f encoder_cast = { static_cast<float>(encoder_v),0,0};encoder_velocity = encoder_velocity + dR*Rbo* encoder_cast*dt;//std::cerr<<"encoder_velocity "<<encoder_velocity;// Compute velocity and position parts of matrices A and B (rely on non-updated delta rotation)// 根据η_ij = A * η_i,j-1 + B_j-1 * η_j-1中的Ａ矩阵和Ｂ矩阵对速度和位移进行更新Eigen::Matrix<float, 3, 3> Wacc = Sophus::SO3f::hat(acc);A.block<3, 3>(3, 0) = -dR * dt * Wacc;A.block<3, 3>(6, 0) = -0.5f * dR * dt * dt * Wacc;A.block<3, 3>(6, 3) = Eigen::DiagonalMatrix<float, 3>(dt, dt, dt);B.block<3, 3>(3, 3) = dR * dt;B.block<3, 3>(6, 3) = 0.5f * dR * dt * dt;// Update position and velocity jacobians wrt bias correction// 因为随着时间推移，不可能每次都重新计算雅克比矩阵，所以需要做J(k+1) = j(k) + (~)这类事，分解方式与AB矩阵相同// 论文作者对forster论文公式的基础上做了变形，然后递归更新，参见 https://github.com/UZ-SLAMLab/ORB_SLAM3/issues/212JPa = JPa + JVa * dt - 0.5f * dR * dt * dt;JPg = JPg + JVg * dt - 0.5f * dR * dt * dt * Wacc * JRg;JVa = JVa - dR * dt;JVg = JVg - dR * dt * Wacc * JRg;// Update delta rotation// Step 2. 构造函数，会根据更新后的bias进行角度积分IntegratedRotation dRi(angVel, b, dt);// 强行归一化使其符合旋转矩阵的格式auto dR_past = dR;dR = NormalizeRotation(dR * dRi.deltaR);//std::cerr<<"dR_past"<<dR_past<<std::endl;//std::cerr<<"dR"<<dR<<std::endl;// Compute rotation parts of matrices A and B// 补充AB矩阵A.block<3, 3>(0, 0) = dRi.deltaR.transpose();B.block<3, 3>(0, 0) = dRi.rightJ * dt;// 小量delta初始为0，更新后通常也为0，故省略了小量的更新// Update covariance// Step 3.更新协方差，frost经典预积分论文的第63个公式，推导了噪声（ηa, ηg）对dR dV dP 的影响C.block<9, 9>(0, 0) = A * C.block<9, 9>(0, 0) * A.transpose() + B * Nga * B.transpose();  // B矩阵为9*6矩阵 Nga 6*6对角矩阵，3个陀螺仪噪声的平方，3个加速度计噪声的平方// 这一部分最开始是0矩阵，随着积分次数增加，每次都加上随机游走，偏置的信息矩阵C.block<6, 6>(9, 9) += NgaWalk;// Update rotation jacobian wrt bias correction// 计算偏置的雅克比矩阵，r对bg的导数，∂ΔRij/∂bg = (ΔRjj-1) * ∂ΔRij-1/∂bg - Jr(j-1)*t// 论文作者对forster论文公式的基础上做了变形，然后递归更新，参见 https://github.com/UZ-SLAMLab/ORB_SLAM3/issues/212// ? 为什么先更新JPa、JPg、JVa、JVg最后更新JRg? 答：这里必须先更新dRi才能更新到这个值，但是为什么JPg和JVg依赖的上一个JRg值进行更新的？JRg = dRi.deltaR.transpose() * JRg - dRi.rightJ * dt;// Total integrated time// 更新总时间dT += dt;bool buseencoder;buseencoder = true;
// TODOif(buseencoder){Eigen::MatrixXf F = Eigen::MatrixXf ::Zero(12,12);
//            F.block<3,3>(0,0) = Eigen::Matrix3f::Identity();
//            F.block<3,3>(0,3) = A.block<3, 3>(6, 0);
//            F.block<3,3>(0,6) = Eigen::DiagonalMatrix<float, 3>(dt, dt, dt);
//
//            F.block<3,3>(3,3) = dRi.deltaR.transpose();
//
//            F.block<3,3>(6,3) = A.block<3, 3>(3, 0);
//            F.block<3,3>(6,6) = Eigen::Matrix3f::Identity();//轮速Eigen::Vector3f encoder_fi = Rbo*encoder_velocity;Eigen::Matrix3f R_encoder;R_encoder<< 0, -encoder_fi(2), encoder_fi(1),encoder_fi(2), 0, -encoder_fi(0),-encoder_fi(1), encoder_fi(0), 0;
//            F.block<3,3>(9,3) =-dR_past*dt*R_encoder;
//            F.block<3,3>(9,9) = Eigen::Matrix3f::Identity();Eigen::MatrixXf V = Eigen::MatrixXf ::Zero(12,9);
//            V.block<3,3>(0,0) = 0.5f * dR_past * dt * dt;
//            V.block<3,3>(3,3) = dRi.rightJ * dt;
//            V.block<3,3>(6,0) = dR_past * dt;
//            V.block<3,3>(9,6) = dR_past*Rbo*dt;F.block<9,9>(0,0) = A;F.block<3,3>(9,3) = -dR_past*dt*R_encoder;F.block<3,3>(9,9) = Eigen::Matrix3f::Identity();V.block<9,6>(0,0) = B;V.block<3,3>(9,6) = dR_past*Rbo*dt;covariance_enc.block<12,12>(0,0) = F*covariance_enc.block<12,12>(0,0)*F.transpose()+V*Nga_en*V.transpose();covariance_enc.block<9,9>(12,12) += NgaWalk_en;}}

1.3 Tracking.cc

…详情请参照古月居

这篇关于ORB-SLAM3如何加入GPS和Wheel轮速约束的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！

---