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 ×tamp, 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/212
JPa = 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;
// TODO
if(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
这部分其实是程序主要的入口，首先是PreintegrateIMU这个函数，这里我们需要再此基础上获取速度信息
Eigen::Vector3f acc, angVel;
double encoder_v;//添加内容
.........
tstep = mvImuFromLastFrame[i+1].t-mCurrentFrame.mpPrevFrame->mTimeStamp;
encoder_v = (mvImuFromLastFrame[i].encoder_v+mvImuFromLastFrame[i+1].encoder_v-
(mvImuFromLastFrame[i+1].encoder_v-mvImuFromLastFrame[i].encoder_v)*(tini/tab))*0.5f;//添加内容
.........
//cerr<<"当前的轮速为： "<< encoder_v<<endl;
bool buseencoder;
buseencoder = true;
if(buseencoder){
mpImuPreintegratedFromLastKF->IntegrateNewMeasurement(acc,angVel,tstep,encoder_v);
pImuPreintegratedFromLastFrame->IntegrateNewMeasurement(acc,angVel,tstep,encoder_v);
}
else{
mpImuPreintegratedFromLastKF->IntegrateNewMeasurement(acc,angVel,tstep);
pImuPreintegratedFromLastFrame->IntegrateNewMeasurement(acc,angVel,tstep);
}
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/212
JPa = 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;
// TODO
if(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;
}
}
然后将对应的特征添加到边中，这里主要是调用InertialOptimization函数，来完成尺度和重力的优化
// Graph edges
// IMU links with gravity and scale
// 6. imu信息链接重力方向与尺度信息
bool buseencoder;
buseencoder = true;
if(!pMap->isImuInitialized()&&buseencoder){ //如果没有第一次初始化并且是使用速
// TODO 这里记得修改
vector<EdgeInertialGSE *> vpei; // 后面虽然加入了边，但是没有用到，应该调试用的
vpei.reserve(vpKFs.size());
vector<pair<KeyFrame *, KeyFrame *>> vppUsedKF;
vppUsedKF.reserve(vpKFs.size()); // 后面虽然加入了关键帧，但是没有用到，应该调试用的
// std::cout << "build optimization graph" << std::endl;
for (size_t i = 0; i < vpKFs.size(); i++)
{
KeyFrame *pKFi = vpKFs[i];
if (pKFi->mPrevKF && pKFi->mnId <= maxKFid)
{
if (pKFi->isBad() || pKFi->mPrevKF->mnId > maxKFid)
continue;
if (!pKFi->mpImuPreintegrated)
std::cout << "Not preintegrated measurement" << std::endl;
// 到这里的条件是pKFi是好的，并且它有上一个关键帧，且他们的id要小于最大id
// 6.1 检查节点指针是否为空
// 将pKFi偏置设定为上一关键帧的偏置
pKFi->mpImuPreintegrated->SetNewBias(pKFi->mPrevKF->GetImuBias());
g2o::HyperGraph::Vertex *VP1 = optimizer.vertex(pKFi->mPrevKF->mnId);
g2o::HyperGraph::Vertex *VV1 = optimizer.vertex(maxKFid + (pKFi->mPrevKF->mnId) + 1);
g2o::HyperGraph::Vertex *VP2 = optimizer.vertex(pKFi->mnId);
g2o::HyperGraph::Vertex *VV2 = optimizer.vertex(maxKFid + (pKFi->mnId) + 1);
g2o::HyperGraph::Vertex *VG = optimizer.vertex(maxKFid * 2 + 2);
g2o::HyperGraph::Vertex *VA = optimizer.vertex(maxKFid * 2 + 3);
g2o::HyperGraph::Vertex *VGDir = optimizer.vertex(maxKFid * 2 + 4);
g2o::HyperGraph::Vertex *VS = optimizer.vertex(maxKFid * 2 + 5);
if (!VP1 || !VV1 || !VG || !VA || !VP2 || !VV2 || !VGDir || !VS)
{
cout << "Error" << VP1 << ", " << VV1 << ", " << VG << ", " << VA << ", " << VP2 << ", " << VV2 << ", " << VGDir << ", " << VS << endl;
continue;
}
// 6.2 这是一个大边。。。。包含了上面所有信息，注意到前面的两个偏置也做了两个一元边加入
// TODO 还要修改这里
if(buseencoder&&(pKFi->mpImuPreintegrated->encoder_velocity[0]==0)) {
cerr << pKFi->mnId << " 的轮速为0 " << endl;
continue;
}
EdgeInertialGSE *ei = new EdgeInertialGSE(pKFi->mpImuPreintegrated);
ei->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VP1));
ei->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VV1));
ei->setVertex(2, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VG));
ei->setVertex(3, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VA));
ei->setVertex(4, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VP2));
ei->setVertex(5, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VV2));
ei->setVertex(6, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VGDir));
ei->setVertex(7, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VS));
vpei.push_back(ei);
vppUsedKF.push_back(make_pair(pKFi->mPrevKF, pKFi));
optimizer.addEdge(ei);
}
}
}
else{
vector<EdgeInertialGS *> vpei; // 后面虽然加入了边，但是没有用到，应该调试用的
vpei.reserve(vpKFs.size());
vector<pair<KeyFrame *, KeyFrame *>> vppUsedKF;
vppUsedKF.reserve(vpKFs.size()); // 后面虽然加入了关键帧，但是没有用到，应该调试用的
// std::cout << "build optimization graph" << std::endl;
for (size_t i = 0; i < vpKFs.size(); i++)
{
KeyFrame *pKFi = vpKFs[i];
if (pKFi->mPrevKF && pKFi->mnId <= maxKFid)
{
if (pKFi->isBad() || pKFi->mPrevKF->mnId > maxKFid)
continue;
if (!pKFi->mpImuPreintegrated)
std::cout << "Not preintegrated measurement" << std::endl;
// 到这里的条件是pKFi是好的，并且它有上一个关键帧，且他们的id要小于最大id
// 6.1 检查节点指针是否为空
// 将pKFi偏置设定为上一关键帧的偏置
pKFi->mpImuPreintegrated->SetNewBias(pKFi->mPrevKF->GetImuBias());
g2o::HyperGraph::Vertex *VP1 = optimizer.vertex(pKFi->mPrevKF->mnId);
g2o::HyperGraph::Vertex *VV1 = optimizer.vertex(maxKFid + (pKFi->mPrevKF->mnId) + 1);
g2o::HyperGraph::Vertex *VP2 = optimizer.vertex(pKFi->mnId);
g2o::HyperGraph::Vertex *VV2 = optimizer.vertex(maxKFid + (pKFi->mnId) + 1);
g2o::HyperGraph::Vertex *VG = optimizer.vertex(maxKFid * 2 + 2);
g2o::HyperGraph::Vertex *VA = optimizer.vertex(maxKFid * 2 + 3);
g2o::HyperGraph::Vertex *VGDir = optimizer.vertex(maxKFid * 2 + 4);
g2o::HyperGraph::Vertex *VS = optimizer.vertex(maxKFid * 2 + 5);
if (!VP1 || !VV1 || !VG || !VA || !VP2 || !VV2 || !VGDir || !VS)
{
cout << "Error" << VP1 << ", " << VV1 << ", " << VG << ", " << VA << ", " << VP2 << ", " << VV2 << ", " << VGDir << ", " << VS << endl;
continue;
}
// 6.2 这是一个大边。。。。包含了上面所有信息，注意到前面的两个偏置也做了两个一元边加入
// TODO 还要修改这里
EdgeInertialGS *ei = new EdgeInertialGS(pKFi->mpImuPreintegrated);
ei->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VP1));
ei->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VV1));
ei->setVertex(2, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VG));
ei->setVertex(3, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VA));
ei->setVertex(4, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VP2));
ei->setVertex(5, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VV2));
ei->setVertex(6, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VGDir));
ei->setVertex(7, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VS));
vpei.push_back(ei);
vppUsedKF.push_back(make_pair(pKFi->mPrevKF, pKFi));
optimizer.addEdge(ei);
}
}
}
// Compute error for different scales
std::set<g2o::HyperGraph::Edge *> setEdges = optimizer.edges();
optimizer.setVerbose(true);
optimizer.initializeOptimization();
optimizer.optimize(its);
对应的边优化在G2oType.h中完成优化。
/**
* @brief 初始化惯性边（误差为残差、加入轮速）
*/
class EdgeInertialGSE : public g2o::BaseMultiEdge<12, Vector12d>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
// EdgeInertialGS(IMU::Preintegrated* pInt);
EdgeInertialGSE(IMU::Preintegrated *pInt);
virtual bool read(std::istream &is) { return false; }
virtual bool write(std::ostream &os) const { return false; }
void computeError();
virtual void linearizeOplus();
const Eigen::Matrix3d JRg, JVg, JPg;
const Eigen::Matrix3d JVa, JPa;
IMU::Preintegrated *mpInt;
const double dt;
Eigen::Vector3d g, gI;
// 关于pose1与2 的旋转平移速度，以及之间的偏置，还有重力方向与尺度的信息矩阵
Eigen::Matrix<double, 27, 27> GetHessian()
{
linearizeOplus();
Eigen::Matrix<double, 12, 27> J;
J.block<12, 6>(0, 0) = _jacobianOplus[0];
J.block<12, 3>(0, 6) = _jacobianOplus[1];
J.block<12, 3>(0, 9) = _jacobianOplus[2];
J.block<12, 3>(0, 12) = _jacobianOplus[3];
J.block<12, 6>(0, 15) = _jacobianOplus[4];
J.block<12, 3>(0, 21) = _jacobianOplus[5];
J.block<12, 2>(0, 24) = _jacobianOplus[6];
J.block<12, 1>(0, 26) = _jacobianOplus[7];
return J.transpose() * information() * J;
}
// 与上面摆放不同
Eigen::Matrix<double, 27, 27> GetHessian2()
{
linearizeOplus();
Eigen::Matrix<double, 12, 27> J;
J.block<12, 3>(0, 0) = _jacobianOplus[2];
J.block<12, 3>(0, 3) = _jacobianOplus[3];
J.block<12, 2>(0, 6) = _jacobianOplus[6];
J.block<12, 1>(0, 8) = _jacobianOplus[7];
J.block<12, 3>(0, 9) = _jacobianOplus[1];
J.block<12, 3>(0, 12) = _jacobianOplus[5];
J.block<12, 6>(0, 15) = _jacobianOplus[0];
J.block<12, 6>(0, 21) = _jacobianOplus[4];
return J.transpose() * information() * J;
}
// 关于偏置，重力方向与尺度的信息矩阵
Eigen::Matrix<double, 12, 12> GetHessian3()
{
linearizeOplus();
Eigen::Matrix<double, 12, 12> J;
J.block<12, 3>(0, 0) = _jacobianOplus[2];
J.block<12, 3>(0, 3) = _jacobianOplus[3];
J.block<12, 2>(0, 6) = _jacobianOplus[6];
J.block<12, 1>(0, 8) = _jacobianOplus[7];
return J.transpose() * information() * J;
}
// 下面的没有用到，其实也是获取状态的信息矩阵
// Eigen::Matrix<double, 1, 1> GetHessianScale()
// {
// linearizeOplus();
// Eigen::Matrix<double, 9, 1> J = _jacobianOplus[7];
// return J.transpose() * information() * J;
// }
//
// Eigen::Matrix<double, 3, 3> GetHessianBiasGyro()
// {
// linearizeOplus();
// Eigen::Matrix<double, 9, 3> J = _jacobianOplus[2];
// return J.transpose() * information() * J;
// }
//
// Eigen::Matrix<double, 3, 3> GetHessianBiasAcc()
// {
// linearizeOplus();
// Eigen::Matrix<double, 9, 3> J = _jacobianOplus[3];
// return J.transpose() * information() * J;
// }
//
// Eigen::Matrix<double, 2, 2> GetHessianGDir()
// {
// linearizeOplus();
// Eigen::Matrix<double, 9, 2> J = _jacobianOplus[6];
// return J.transpose() * information() * J;
// }
};
2. GPS约束
2.1 Map2DFusion
这种方法是将GPS结果和VSLAM结果相结合。这部分是使用GPS定位去优化视觉SLAM。相对来说GPS是低精度高延迟累积误差小，视觉SLAM是高精度低延迟累积误差大。GPS适合用来减小视觉SLAM的累积误差(或许可以用在回环检测中)
2.2 gnss-stereo-inertial-fusion
这一部分是使用GPS做紧耦合，通过将GPS测量融合到ORB-SLAM3中，以提供更准确的精度。
评论（0）
您还未登录，请登录后发表或查看评论