2021SC@SDUSC
1.前言
这一部分代码量巨大，查阅了很多资料结合来看的代码，将分为以下部分进行分析
- 单帧优化
- 局部地图优化
- 全局优化
- 尺度与重力优化
- sim3优化
- 地图回环优化
- 地图融合优化
下面给出逐步注释分析
2.代码分析
全局BA： pMap中所有的MapPoints和关键帧做bundle adjustment优化
这个全局BA优化在本程序中有两个地方使用：
- 1、单目初始化：CreateInitialMapMonocular函数
- 2、闭环优化：RunGlobalBundleAdjustment函数
void Optimizer::GlobalBundleAdjustemnt(Map *pMap, int nIterations, bool *pbStopFlag, const unsigned long nLoopKF, const bool bRobust)
{
// 获取地图中的所有关键帧
vector<KeyFrame *> vpKFs = pMap->GetAllKeyFrames();
// 获取地图中的所有地图点
vector<MapPoint *> vpMP = pMap->GetAllMapPoints();
// 调用GBA
BundleAdjustment(vpKFs, vpMP, nIterations, pbStopFlag, nLoopKF, bRobust);
}
bundle adjustment 优化过程
void Optimizer::BundleAdjustment(const vector<KeyFrame *> &vpKFs, const vector<MapPoint *> &vpMP,
int nIterations, bool *pbStopFlag, const unsigned long nLoopKF, const bool bRobust)
{
// 不参与优化的地图点，下面会用到
vector<bool> vbNotIncludedMP;
vbNotIncludedMP.resize(vpMP.size());
Map *pMap = vpKFs[0]->GetMap();
// Step 1 初始化g2o优化器
g2o::SparseOptimizer optimizer;
g2o::BlockSolver_6_3::LinearSolverType *linearSolver;
linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
g2o::BlockSolver_6_3 *solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
// 使用LM算法优化
g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
optimizer.setAlgorithm(solver);
optimizer.setVerbose(false);
// 如果这个时候外部请求终止，那就结束
// 注意这句执行之后，外部再请求结束BA，就结束不了了
if (pbStopFlag)
optimizer.setForceStopFlag(pbStopFlag);
// 记录添加到优化器中的顶点的最大关键帧id
long unsigned int maxKFid = 0;
const int nExpectedSize = (vpKFs.size()) * vpMP.size();
vector<ORB_SLAM3::EdgeSE3ProjectXYZ *> vpEdgesMono;
vpEdgesMono.reserve(nExpectedSize);
vector<ORB_SLAM3::EdgeSE3ProjectXYZToBody *> vpEdgesBody;
vpEdgesBody.reserve(nExpectedSize);
vector<KeyFrame *> vpEdgeKFMono;
vpEdgeKFMono.reserve(nExpectedSize);
vector<KeyFrame *> vpEdgeKFBody;
vpEdgeKFBody.reserve(nExpectedSize);
vector<MapPoint *> vpMapPointEdgeMono;
vpMapPointEdgeMono.reserve(nExpectedSize);
vector<MapPoint *> vpMapPointEdgeBody;
vpMapPointEdgeBody.reserve(nExpectedSize);
vector<g2o::EdgeStereoSE3ProjectXYZ *> vpEdgesStereo;
vpEdgesStereo.reserve(nExpectedSize);
vector<KeyFrame *> vpEdgeKFStereo;
vpEdgeKFStereo.reserve(nExpectedSize);
vector<MapPoint *> vpMapPointEdgeStereo;
vpMapPointEdgeStereo.reserve(nExpectedSize);
// Step 2 向优化器添加顶点
// Set KeyFrame vertices
// Step 2.1 ：向优化器添加关键帧位姿顶点
// 对于当前地图中的所有关键帧
for (size_t i = 0; i < vpKFs.size(); i++)
{
KeyFrame *pKF = vpKFs[i];
// 去除无效的
if (pKF->isBad())
continue;
// 对于每一个能用的关键帧构造SE3顶点,其实就是当前关键帧的位姿
g2o::VertexSE3Expmap *vSE3 = new g2o::VertexSE3Expmap();
vSE3->setEstimate(Converter::toSE3Quat(pKF->GetPose()));
vSE3->setId(pKF->mnId);
// 只有第0帧关键帧不优化（参考基准）
vSE3->setFixed(pKF->mnId == pMap->GetInitKFid());
// 向优化器中添加顶点，并且更新maxKFid
optimizer.addVertex(vSE3);
if (pKF->mnId > maxKFid)
maxKFid = pKF->mnId;
}
// 卡方分布 95% 以上可信度的时候的阈值
const float thHuber2D = sqrt(5.99); // 自由度为2
const float thHuber3D = sqrt(7.815); // 自由度为3
// Set MapPoint vertices
// Step 2.2：向优化器添加MapPoints顶点
// 遍历地图中的所有地图点
for (size_t i = 0; i < vpMP.size(); i++)
{
MapPoint *pMP = vpMP[i];
// 跳过无效地图点
if (pMP->isBad())
continue;
// 创建顶点
g2o::VertexSBAPointXYZ *vPoint = new g2o::VertexSBAPointXYZ();
// 注意由于地图点的位置是使用cv::Mat数据类型表示的,这里需要转换成为Eigen::Vector3d类型
vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
// 前面记录maxKFid 是在这里使用的
const int id = pMP->mnId + maxKFid + 1;
vPoint->setId(id);
// g2o在做BA的优化时必须将其所有地图点全部schur掉，否则会出错。
// 原因是使用了g2o::LinearSolver<BalBlockSolver::PoseMatrixType>这个类型来指定linearsolver,
// 其中模板参数当中的位姿矩阵类型在程序中为相机姿态参数的维度，于是BA当中schur消元后解得线性方程组必须是只含有相机姿态变量。
// Ceres库则没有这样的限制
vPoint->setMarginalized(true);
optimizer.addVertex(vPoint);
// 边的关系，其实就是点和关键帧之间观测的关系
const map<KeyFrame *, tuple<int, int>> observations = pMP->GetObservations();
// 边计数
int nEdges = 0;
// SET EDGES
// Step 3：向优化器添加投影边（是在遍历地图点、添加地图点的顶点的时候顺便添加的）
// 遍历观察到当前地图点的所有关键帧
for (map<KeyFrame *, tuple<int, int>>::const_iterator mit = observations.begin(); mit != observations.end(); mit++)
{
KeyFrame *pKF = mit->first;
// 滤出不合法的关键帧
if (pKF->isBad() || pKF->mnId > maxKFid)
continue;
if (optimizer.vertex(id) == NULL || optimizer.vertex(pKF->mnId) == NULL)
continue;
nEdges++;
const int leftIndex = get<0>(mit->second);
if (leftIndex != -1 && pKF->mvuRight[get<0>(mit->second)] < 0)
{
// 如果是单目相机按照下面操作
const cv::KeyPoint &kpUn = pKF->mvKeysUn[leftIndex];
Eigen::Matrix<double, 2, 1> obs;
obs << kpUn.pt.x, kpUn.pt.y;
// 创建边
ORB_SLAM3::EdgeSE3ProjectXYZ *e = new ORB_SLAM3::EdgeSE3ProjectXYZ();
// 填充数据，构造约束关系
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(id)));
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(pKF->mnId)));
e->setMeasurement(obs);
// 信息矩阵，也是协方差，表明了这个约束的观测在各个维度（x,y）上的可信程度，在我们这里对于具体的一个点，两个坐标的可信程度都是相同的，
// 其可信程度受到特征点在图像金字塔中的图层有关，图层越高，可信度越差
// 为了避免出现信息矩阵中元素为负数的情况，这里使用的是sigma^(-2)
const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave];
e->setInformation(Eigen::Matrix2d::Identity() * invSigma2);
// 如果要使用鲁棒核函数
if (bRobust)
{
g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
// 这里的重投影误差，自由度为2，所以这里设置为卡方分布中自由度为2的阈值，如果重投影的误差大约大于1个像素的时候，就认为不太靠谱的点了，
// 核函数是为了避免其误差的平方项出现数值上过大的增长
rk->setDelta(thHuber2D);
}
// 设置相机内参
e->pCamera = pKF->mpCamera;
optimizer.addEdge(e);
vpEdgesMono.push_back(e);
vpEdgeKFMono.push_back(pKF);
vpMapPointEdgeMono.push_back(pMP);
}
else if (leftIndex != -1 && pKF->mvuRight[leftIndex] >= 0) // Stereo observation
{
// 双目或RGBD相机按照下面操作
// 双目相机的观测数据则是由三个部分组成：投影点的x坐标，投影点的y坐标，以及投影点在右目中的x坐标（默认y方向上已经对齐了）
const cv::KeyPoint &kpUn = pKF->mvKeysUn[leftIndex];
Eigen::Matrix<double, 3, 1> obs;
const float kp_ur = pKF->mvuRight[get<0>(mit->second)];
obs << kpUn.pt.x, kpUn.pt.y, kp_ur;
// 对于双目输入，g2o也有专门的误差边
g2o::EdgeStereoSE3ProjectXYZ *e = new g2o::EdgeStereoSE3ProjectXYZ();
// 填充
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(id)));
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(pKF->mnId)));
e->setMeasurement(obs);
// 信息矩阵这里是相同的，考虑的是左目特征点的所在图层
const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave];
Eigen::Matrix3d Info = Eigen::Matrix3d::Identity() * invSigma2;
e->setInformation(Info);
// 如果要使用鲁棒核函数
if (bRobust)
{
g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
// 由于现在的观测有三个值，重投影误差会有三个平方项的和组成，因此对应的卡方分布的自由度为3，所以这里设置的也是自由度为3的时候的阈值
rk->setDelta(thHuber3D);
}
// 填充相机的基本参数
e->fx = pKF->fx;
e->fy = pKF->fy;
e->cx = pKF->cx;
e->cy = pKF->cy;
e->bf = pKF->mbf;
optimizer.addEdge(e);
vpEdgesStereo.push_back(e);
vpEdgeKFStereo.push_back(pKF);
vpMapPointEdgeStereo.push_back(pMP);
}
if (pKF->mpCamera2)
{
int rightIndex = get<1>(mit->second);
if (rightIndex != -1 && rightIndex < pKF->mvKeysRight.size())
{
rightIndex -= pKF->NLeft;
Eigen::Matrix<double, 2, 1> obs;
cv::KeyPoint kp = pKF->mvKeysRight[rightIndex];
obs << kp.pt.x, kp.pt.y;
ORB_SLAM3::EdgeSE3ProjectXYZToBody *e = new ORB_SLAM3::EdgeSE3ProjectXYZToBody();
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(id)));
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(pKF->mnId)));
e->setMeasurement(obs);
const float &invSigma2 = pKF->mvInvLevelSigma2[kp.octave];
e->setInformation(Eigen::Matrix2d::Identity() * invSigma2);
g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
rk->setDelta(thHuber2D);
e->mTrl = Converter::toSE3Quat(pKF->mTrl);
e->pCamera = pKF->mpCamera2;
optimizer.addEdge(e);
vpEdgesBody.push_back(e);
vpEdgeKFBody.push_back(pKF);
vpMapPointEdgeBody.push_back(pMP);
}
}
}
// 如果因为一些特殊原因,实际上并没有任何关键帧观测到当前的这个地图点,那么就删除掉这个顶点,并且这个地图点也就不参与优化
if (nEdges == 0)
{
optimizer.removeVertex(vPoint);
vbNotIncludedMP[i] = true;
}
else
{
vbNotIncludedMP[i] = false;
}
}
// Optimize!
// Step 4：开始优化
optimizer.setVerbose(false);
optimizer.initializeOptimization();
optimizer.optimize(nIterations);
Verbose::PrintMess("BA: End of the optimization", Verbose::VERBOSITY_NORMAL);
// Recover optimized data
// Step 5：得到优化的结果
// Step 5.1 Keyframes
// 遍历所有的关键帧
for (size_t i = 0; i < vpKFs.size(); i++)
{
KeyFrame *pKF = vpKFs[i];
if (pKF->isBad())
continue;
// 获取到优化结果
g2o::VertexSE3Expmap *vSE3 = static_cast<g2o::VertexSE3Expmap *>(optimizer.vertex(pKF->mnId));
g2o::SE3Quat SE3quat = vSE3->estimate();
if (nLoopKF == pMap->GetOriginKF()->mnId)
{
// 原则上来讲不会出现"当前闭环关键帧是第0帧"的情况,如果这种情况出现,只能够说明是在创建初始地图点的时候调用的这个全局BA函数.
// 这个时候,地图中就只有两个关键帧,其中优化后的位姿数据可以直接写入到帧的成员变量中
// 老白：视觉初始化时
pKF->SetPose(Converter::toCvMat(SE3quat));
}
else
{
// 正常的操作,先把优化后的位姿写入到帧的一个专门的成员变量mTcwGBA中备用
pKF->mTcwGBA.create(4, 4, CV_32F);
Converter::toCvMat(SE3quat).copyTo(pKF->mTcwGBA);
pKF->mnBAGlobalForKF = nLoopKF; // 标记这个关键帧参与了这次全局优化
// 下面都是一些调试操作，计算优化前后的位移
cv::Mat mTwc = pKF->GetPoseInverse();
cv::Mat mTcGBA_c = pKF->mTcwGBA * mTwc;
cv::Vec3d vector_dist = mTcGBA_c.rowRange(0, 3).col(3);
double dist = cv::norm(vector_dist);
if (dist > 1)
{
int numMonoBadPoints = 0, numMonoOptPoints = 0;
int numStereoBadPoints = 0, numStereoOptPoints = 0;
vector<MapPoint *> vpMonoMPsOpt, vpStereoMPsOpt;
for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++)
{
ORB_SLAM3::EdgeSE3ProjectXYZ *e = vpEdgesMono[i];
MapPoint *pMP = vpMapPointEdgeMono[i];
KeyFrame *pKFedge = vpEdgeKFMono[i];
if (pKF != pKFedge)
{
continue;
}
if (pMP->isBad())
continue;
if (e->chi2() > 5.991 || !e->isDepthPositive())
{
numMonoBadPoints++;
}
else
{
numMonoOptPoints++;
vpMonoMPsOpt.push_back(pMP);
}
}
for (size_t i = 0, iend = vpEdgesStereo.size(); i < iend; i++)
{
g2o::EdgeStereoSE3ProjectXYZ *e = vpEdgesStereo[i];
MapPoint *pMP = vpMapPointEdgeStereo[i];
KeyFrame *pKFedge = vpEdgeKFMono[i];
if (pKF != pKFedge)
{
continue;
}
if (pMP->isBad())
continue;
if (e->chi2() > 7.815 || !e->isDepthPositive())
{
numStereoBadPoints++;
}
else
{
numStereoOptPoints++;
vpStereoMPsOpt.push_back(pMP);
}
}
}
}
}
// Step 5.2 遍历所有地图点,去除其中没有参与优化过程的地图点
for (size_t i = 0; i < vpMP.size(); i++)
{
if (vbNotIncludedMP[i])
continue;
MapPoint *pMP = vpMP[i];
if (pMP->isBad())
continue;
// 获取优化之后的地图点的位置
g2o::VertexSBAPointXYZ *vPoint = static_cast<g2o::VertexSBAPointXYZ *>(optimizer.vertex(pMP->mnId + maxKFid + 1));
if (nLoopKF == pMap->GetOriginKF()->mnId)
{
// 如果这个GBA是在创建初始地图的时候调用的话,那么地图点的位姿也可以直接写入
pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate()));
pMP->UpdateNormalAndDepth();
}
else
{
// 反之,如果是正常的闭环过程调用,就先临时保存一下
pMP->mPosGBA.create(3, 1, CV_32F);
Converter::toCvMat(vPoint->estimate()).copyTo(pMP->mPosGBA);
pMP->mnBAGlobalForKF = nLoopKF;
}
}
}
brief imu初始化优化，LocalMapping::InitializeIMU中使用 LoopClosing::RunGlobalBundleAdjustment
地图全部做BA。也就是imu版的GlobalBundleAdjustemnt
void Optimizer::FullInertialBA(Map *pMap, int its, const bool bFixLocal, const long unsigned int nLoopId, bool *pbStopFlag, bool bInit, float priorG, float priorA, Eigen::VectorXd *vSingVal, bool *bHess)
{
// 获取地图里所有mp与kf，以及最大kf的id
long unsigned int maxKFid = pMap->GetMaxKFid();
const vector<KeyFrame *> vpKFs = pMap->GetAllKeyFrames();
const vector<MapPoint *> vpMPs = pMap->GetAllMapPoints();
// Setup optimizer
// 1. 很经典的一套设置优化器流程
g2o::SparseOptimizer optimizer;
g2o::BlockSolverX::LinearSolverType *linearSolver;
linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolverX::PoseMatrixType>();
g2o::BlockSolverX *solver_ptr = new g2o::BlockSolverX(linearSolver);
g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
solver->setUserLambdaInit(1e-5);
optimizer.setAlgorithm(solver);
optimizer.setVerbose(false);
if (pbStopFlag)
optimizer.setForceStopFlag(pbStopFlag);
int nNonFixed = 0;
// 2. 设置关键帧与偏置节点
// Set KeyFrame vertices
KeyFrame *pIncKF; // vpKFs中最后一个id符合要求的关键帧
for (size_t i = 0; i < vpKFs.size(); i++)
{
KeyFrame *pKFi = vpKFs[i];
if (pKFi->mnId > maxKFid)
continue;
VertexPose *VP = new VertexPose(pKFi);
VP->setId(pKFi->mnId);
pIncKF = pKFi;
bool bFixed = false;
if (bFixLocal)
{
bFixed = (pKFi->mnBALocalForKF >= (maxKFid - 1)) || (pKFi->mnBAFixedForKF >= (maxKFid - 1));
if (!bFixed)
nNonFixed++;
VP->setFixed(bFixed); // 固定，这里可能跟回环有关
}
optimizer.addVertex(VP);
// 如果是初始化的那几个及后面的关键帧，加入速度节点
if (pKFi->bImu)
{
VertexVelocity *VV = new VertexVelocity(pKFi);
VV->setId(maxKFid + 3 * (pKFi->mnId) + 1);
VV->setFixed(bFixed);
optimizer.addVertex(VV);
// priorA==0.f 时 bInit为false，也就是又加入了偏置节点
if (!bInit)
{
VertexGyroBias *VG = new VertexGyroBias(pKFi);
VG->setId(maxKFid + 3 * (pKFi->mnId) + 2);
VG->setFixed(bFixed);
optimizer.addVertex(VG);
VertexAccBias *VA = new VertexAccBias(pKFi);
VA->setId(maxKFid + 3 * (pKFi->mnId) + 3);
VA->setFixed(bFixed);
optimizer.addVertex(VA);
}
}
}
// priorA!=0.f 时 bInit为true，加入了偏置节点
if (bInit)
{
VertexGyroBias *VG = new VertexGyroBias(pIncKF);
VG->setId(4 * maxKFid + 2);
VG->setFixed(false);
optimizer.addVertex(VG);
VertexAccBias *VA = new VertexAccBias(pIncKF);
VA->setId(4 * maxKFid + 3);
VA->setFixed(false);
optimizer.addVertex(VA);
}
// false
if (bFixLocal)
{
if (nNonFixed < 3)
return;
}
// 3. 添加关于imu的边
// IMU links
for (size_t i = 0; i < vpKFs.size(); i++)
{
KeyFrame *pKFi = vpKFs[i];
// 必须有对应的上一个关键帧，感觉跟下面的判定冲突了
if (!pKFi->mPrevKF)
{
Verbose::PrintMess("NOT INERTIAL LINK TO PREVIOUS FRAME!", Verbose::VERBOSITY_NORMAL);
continue;
}
if (pKFi->mPrevKF && pKFi->mnId <= maxKFid)
{
if (pKFi->isBad() || pKFi->mPrevKF->mnId > maxKFid)
continue;
// 这两个都必须为初始化后的关键帧
if (pKFi->bImu && pKFi->mPrevKF->bImu)
{
// 3.1 根据上一帧的偏置设定当前帧的新偏置
pKFi->mpImuPreintegrated->SetNewBias(pKFi->mPrevKF->GetImuBias());
// 3.2 提取节点
g2o::HyperGraph::Vertex *VP1 = optimizer.vertex(pKFi->mPrevKF->mnId);
g2o::HyperGraph::Vertex *VV1 = optimizer.vertex(maxKFid + 3 * (pKFi->mPrevKF->mnId) + 1);
g2o::HyperGraph::Vertex *VG1;
g2o::HyperGraph::Vertex *VA1;
g2o::HyperGraph::Vertex *VG2;
g2o::HyperGraph::Vertex *VA2;
// 根据不同输入配置相应的偏置节点
if (!bInit)
{
VG1 = optimizer.vertex(maxKFid + 3 * (pKFi->mPrevKF->mnId) + 2);
VA1 = optimizer.vertex(maxKFid + 3 * (pKFi->mPrevKF->mnId) + 3);
VG2 = optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 2);
VA2 = optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 3);
}
else
{
VG1 = optimizer.vertex(4 * maxKFid + 2);
VA1 = optimizer.vertex(4 * maxKFid + 3);
}
g2o::HyperGraph::Vertex *VP2 = optimizer.vertex(pKFi->mnId);
g2o::HyperGraph::Vertex *VV2 = optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 1);
if (!bInit)
{
if (!VP1 || !VV1 || !VG1 || !VA1 || !VP2 || !VV2 || !VG2 || !VA2)
{
cout << "Error" << VP1 << ", " << VV1 << ", " << VG1 << ", " << VA1 << ", " << VP2 << ", " << VV2 << ", " << VG2 << ", " << VA2 << endl;
continue;
}
}
else
{
if (!VP1 || !VV1 || !VG1 || !VA1 || !VP2 || !VV2)
{
cout << "Error" << VP1 << ", " << VV1 << ", " << VG1 << ", " << VA1 << ", " << VP2 << ", " << VV2 << endl;
continue;
}
}
// 3.3 设置边
EdgeInertial *ei = new EdgeInertial(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 *>(VG1));
ei->setVertex(3, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VA1));
ei->setVertex(4, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VP2));
ei->setVertex(5, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VV2));
g2o::RobustKernelHuber *rki = new g2o::RobustKernelHuber;
ei->setRobustKernel(rki);
// 9个自由度的卡方检验（0.05）
rki->setDelta(sqrt(16.92));
optimizer.addEdge(ei);
// 加了每一个关键帧的偏置时，还要优化两帧之间偏置的误差
if (!bInit)
{
EdgeGyroRW *egr = new EdgeGyroRW();
egr->setVertex(0, VG1);
egr->setVertex(1, VG2);
cv::Mat cvInfoG = pKFi->mpImuPreintegrated->C.rowRange(9, 12).colRange(9, 12).inv(cv::DECOMP_SVD);
Eigen::Matrix3d InfoG;
for (int r = 0; r < 3; r++)
for (int c = 0; c < 3; c++)
InfoG(r, c) = cvInfoG.at<float>(r, c);
egr->setInformation(InfoG);
egr->computeError();
optimizer.addEdge(egr);
EdgeAccRW *ear = new EdgeAccRW();
ear->setVertex(0, VA1);
ear->setVertex(1, VA2);
cv::Mat cvInfoA = pKFi->mpImuPreintegrated->C.rowRange(12, 15).colRange(12, 15).inv(cv::DECOMP_SVD);
Eigen::Matrix3d InfoA;
for (int r = 0; r < 3; r++)
for (int c = 0; c < 3; c++)
InfoA(r, c) = cvInfoA.at<float>(r, c);
ear->setInformation(InfoA);
ear->computeError();
optimizer.addEdge(ear);
}
}
else
{
cout << pKFi->mnId << " or " << pKFi->mPrevKF->mnId << " no imu" << endl;
}
}
}
// 只加入pIncKF帧的偏置，优化偏置到0
if (bInit)
{
g2o::HyperGraph::Vertex *VG = optimizer.vertex(4 * maxKFid + 2);
g2o::HyperGraph::Vertex *VA = optimizer.vertex(4 * maxKFid + 3);
// Add prior to comon biases
EdgePriorAcc *epa = new EdgePriorAcc(cv::Mat::zeros(3, 1, CV_32F));
epa->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VA));
double infoPriorA = priorA; //
epa->setInformation(infoPriorA * Eigen::Matrix3d::Identity());
optimizer.addEdge(epa);
EdgePriorGyro *epg = new EdgePriorGyro(cv::Mat::zeros(3, 1, CV_32F));
epg->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(VG));
double infoPriorG = priorG; //
epg->setInformation(infoPriorG * Eigen::Matrix3d::Identity());
optimizer.addEdge(epg);
}
const float thHuberMono = sqrt(5.991);
const float thHuberStereo = sqrt(7.815);
const unsigned long iniMPid = maxKFid * 5;
vector<bool> vbNotIncludedMP(vpMPs.size(), false);
// 5. 添加关于mp的节点与边，这段比较好理解，很传统的视觉上的重投影误差
for (size_t i = 0; i < vpMPs.size(); i++)
{
MapPoint *pMP = vpMPs[i];
g2o::VertexSBAPointXYZ *vPoint = new g2o::VertexSBAPointXYZ();
vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
unsigned long id = pMP->mnId + iniMPid + 1;
vPoint->setId(id);
vPoint->setMarginalized(true);
optimizer.addVertex(vPoint);
const map<KeyFrame *, tuple<int, int>> observations = pMP->GetObservations();
bool bAllFixed = true;
// Set edges
// 遍历所有能观测到这个点的关键帧
for (map<KeyFrame *, tuple<int, int>>::const_iterator mit = observations.begin(), mend = observations.end(); mit != mend; mit++)
{
KeyFrame *pKFi = mit->first;
if (pKFi->mnId > maxKFid)
continue;
if (!pKFi->isBad())
{
const int leftIndex = get<0>(mit->second);
cv::KeyPoint kpUn;
// 添加边
if (leftIndex != -1 && pKFi->mvuRight[get<0>(mit->second)] < 0) // Monocular observation
{
kpUn = pKFi->mvKeysUn[leftIndex];
Eigen::Matrix<double, 2, 1> obs;
obs << kpUn.pt.x, kpUn.pt.y;
EdgeMono *e = new EdgeMono(0);
g2o::OptimizableGraph::Vertex *VP = dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(pKFi->mnId));
if (bAllFixed)
if (!VP->fixed())
bAllFixed = false;
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(id)));
e->setVertex(1, VP);
e->setMeasurement(obs);
const float invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave];
e->setInformation(Eigen::Matrix2d::Identity() * invSigma2);
g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
rk->setDelta(thHuberMono);
optimizer.addEdge(e);
}
else if (leftIndex != -1 && pKFi->mvuRight[leftIndex] >= 0) // stereo observation
{
kpUn = pKFi->mvKeysUn[leftIndex];
const float kp_ur = pKFi->mvuRight[leftIndex];
Eigen::Matrix<double, 3, 1> obs;
obs << kpUn.pt.x, kpUn.pt.y, kp_ur;
EdgeStereo *e = new EdgeStereo(0);
g2o::OptimizableGraph::Vertex *VP = dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(pKFi->mnId));
if (bAllFixed)
if (!VP->fixed())
bAllFixed = false;
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(id)));
e->setVertex(1, VP);
e->setMeasurement(obs);
const float invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave];
e->setInformation(Eigen::Matrix3d::Identity() * invSigma2);
g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
rk->setDelta(thHuberStereo);
optimizer.addEdge(e);
}
if (pKFi->mpCamera2)
{ // Monocular right observation
int rightIndex = get<1>(mit->second);
if (rightIndex != -1 && rightIndex < pKFi->mvKeysRight.size())
{
rightIndex -= pKFi->NLeft;
Eigen::Matrix<double, 2, 1> obs;
kpUn = pKFi->mvKeysRight[rightIndex];
obs << kpUn.pt.x, kpUn.pt.y;
EdgeMono *e = new EdgeMono(1);
g2o::OptimizableGraph::Vertex *VP = dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(pKFi->mnId));
if (bAllFixed)
if (!VP->fixed())
bAllFixed = false;
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(id)));
e->setVertex(1, VP);
e->setMeasurement(obs);
const float invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave];
e->setInformation(Eigen::Matrix2d::Identity() * invSigma2);
g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
rk->setDelta(thHuberMono);
optimizer.addEdge(e);
}
}
}
}
// false
if (bAllFixed)
{
optimizer.removeVertex(vPoint);
vbNotIncludedMP[i] = true;
}
}
if (pbStopFlag)
if (*pbStopFlag)
return;
optimizer.initializeOptimization();
optimizer.optimize(its);
// 5. 取出优化结果，对应的值赋值
// Recover optimized data
// Keyframes
for (size_t i = 0; i < vpKFs.size(); i++)
{
KeyFrame *pKFi = vpKFs[i];
if (pKFi->mnId > maxKFid)
continue;
VertexPose *VP = static_cast<VertexPose *>(optimizer.vertex(pKFi->mnId));
if (nLoopId == 0)
{
cv::Mat Tcw = Converter::toCvSE3(VP->estimate().Rcw[0], VP->estimate().tcw[0]);
pKFi->SetPose(Tcw);
}
else
{
pKFi->mTcwGBA = cv::Mat::eye(4, 4, CV_32F);
Converter::toCvMat(VP->estimate().Rcw[0]).copyTo(pKFi->mTcwGBA.rowRange(0, 3).colRange(0, 3));
Converter::toCvMat(VP->estimate().tcw[0]).copyTo(pKFi->mTcwGBA.rowRange(0, 3).col(3));
pKFi->mnBAGlobalForKF = nLoopId;
}
if (pKFi->bImu)
{
VertexVelocity *VV = static_cast<VertexVelocity *>(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 1));
if (nLoopId == 0)
{
pKFi->SetVelocity(Converter::toCvMat(VV->estimate()));
}
else
{
pKFi->mVwbGBA = Converter::toCvMat(VV->estimate());
}
VertexGyroBias *VG;
VertexAccBias *VA;
if (!bInit)
{
VG = static_cast<VertexGyroBias *>(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 2));
VA = static_cast<VertexAccBias *>(optimizer.vertex(maxKFid + 3 * (pKFi->mnId) + 3));
}
else
{
VG = static_cast<VertexGyroBias *>(optimizer.vertex(4 * maxKFid + 2));
VA = static_cast<VertexAccBias *>(optimizer.vertex(4 * maxKFid + 3));
}
Vector6d vb;
vb << VG->estimate(), VA->estimate();
IMU::Bias b(vb[3], vb[4], vb[5], vb[0], vb[1], vb[2]);
if (nLoopId == 0)
{
pKFi->SetNewBias(b);
}
else
{
pKFi->mBiasGBA = b;
}
}
}
// Points
for (size_t i = 0; i < vpMPs.size(); i++)
{
if (vbNotIncludedMP[i])
continue;
MapPoint *pMP = vpMPs[i];
g2o::VertexSBAPointXYZ *vPoint = static_cast<g2o::VertexSBAPointXYZ *>(optimizer.vertex(pMP->mnId + iniMPid + 1));
if (nLoopId == 0)
{
pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate()));
pMP->UpdateNormalAndDepth();
}
else
{
pMP->mPosGBA.create(3, 1, CV_32F);
Converter::toCvMat(vPoint->estimate()).copyTo(pMP->mPosGBA);
pMP->mnBAGlobalForKF = nLoopId;
}
}
pMap->IncreaseChangeIndex();
}
扫一扫
2240
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
