0. 简介
由于多相机之间通常存在有限或无重叠的视场，因此在估计外参相机参数时面临着一定的挑战，为了解决这个问题，本文提出了CamMap：一种新颖的6自由度外参标定流程。根据三个操作规则，使一个多相机系统单独捕捉一些类似的图像序列，以使用SLAM系统创建基于稀疏特征的地图。我们构建了一个两阶段的优化问题来对齐这些地图，并基于双向投影得到它们之间的变换，这些变换即为外参参数。该方案支持各种相机类型，在任何纹理丰富的环境中都可以使用。它可以同时标定任意数量的相机，无需标定板、同步、相同分辨率和频率。我们对具有有限和无重叠视场的相机进行了实验评估，结果显示我们的方法具有较高的准确性和效率。Kalibr和CamMap之间的绝对姿态误差（APE）小于0.025。代码已经在Github上开源了。我们这里文字主要是基于《CamMap：基于SLAM地图对不共视相机进行外参标定》的，当中插入代码以供学习。
1. 主要贡献
准确估计相机外参对于关联多个相机的信息至关重要。然而，如图1(a)所示，重叠视场通常很小或不存在，这给外参标定带来了重大挑战。为了让标定板能够被同时捕捉到，标定板必须远离相机放置。由于角点提取像素误差的增加，标定精度会降低，对于没有重叠视场的情况，需要一个充满已知相对位置模式的标定房间，这既昂贵又不方便。
图1. 非重叠相机的组合示例。(a) 配备两个RealSense D455和两个L515的TurtleBot。我们移除了连接线以清晰显示。(b) RealSense D455的组成，右侧图像传感器和左侧图像传感器可以组合成立体摄像头，并且左侧图像传感器的坐标系被用作参考，中间的RGB模块可以单独用作单目摄像头。(c) RealSense L515的结构，它可以用作RGB-D或单目摄像头。
针对解决上述问题并充分利用ORB-SLAM3，本文提出了CamMap：一种6自由度外参标定流程，它可以对非重叠相机进行高精度的外参标定，并可应用于多种类型的相机，如单目、立体和RGB-D相机。该标定方法CamMap通过对ORB-SLAM3创建的地图进行对齐来实现，换句话说，自然场景可以用作标定模式。在分别使用两个相机构建两个相似地图并找到所有匹配的地图点后，外参参数正好是地图之间的变换关系。标定过程仅耗费几十秒时间。所提出的方法的应用要求在表I中可以找到。
本文的主要贡献如下：
-
提出了一种相机外参标定流程，将ORB-SLAM3系统集成到不重叠视场的各种相机类型中，在任何纹理丰富的自然环境中都可以使用，无需标定板，并且可以同时标定任意数量的相机。并将所提出的方法开源。
-
为不同位置的多相机提出了三个操作规则，这旨在消除相机不同步时的理论误差，并减少SLAM漂移引起的误差。
-
我们在两阶段优化问题中引入了基于双向投影的代价函数来计算外参参数，然后提供了一种用于确定标定是否成功的SLAM漂移评估方法。
2. 主要内容
2.1 概述
这里详细描述了所提出的方法，如图2所示，一个装有两个刚性连接相机A和B的支架，在支架上进行一系列规定的运动后，相机捕捉图像流，这些图像流将由ORB-SLAM3处理，用于创建地图，包括关键帧A_i、B_j（其中i = 1，…，m，j = 1，…，n）和地图点P，值得注意的是地图坐标系与相机的第一个关键帧坐标系重合，这意味着地图之间的变换T^{Map}_{BA}实际上是A_1和B_1之间的变换T^{B_1}_{A_1}，也即是外参参数。然后执行类似关键帧检测以在两个地图中找到匹配的关键帧。最后使用两阶段优化通过对所有匹配地图点对（P_k^{A_1}，P_k^{B_1}）进行对齐来估计外参参数（其中k ∈ N，N = {1，…，l}$$）。CamMap的流程包括以下步骤：
-
通过相机捕捉一些图像序列，当多相机支架进行一组固定运动时，例如在小圆圈上旋转，这应该根据相机的相对位置进行规划，在标定开始和结束时，支架应保持静止。
-
使用ORB-SLAM3系统处理图像序列，以创建基于ORB特征的地图。
-
使用词袋（BoW）模块对两个地图之间的所有关键帧进行相似性检测，找到相似关键帧并匹配地图点。
-
对相似关键帧捕捉的尺度和局部地图进行对齐，以估计外参参数，这个过程是“帧到帧”的对齐，是第一个优化阶段。同时，使用卡方检验来删除错误匹配的地图点对。
-
对于第二阶段的优化，使用所有正确匹配的地图点对来优化外参参数，这是“地图对地图”的对齐。卡方检验用于获得内点的数量。最后根据内点数量和T^{A_1}_{B_1}与T^{A_n}_{B_m}之间的差异判断标定是否成功。
// Create SLAM system. It initializes all system threads and gets ready to process frames.
ORB_SLAM3::System SLAM1(argv[1], argv[3], static_cast<ORB_SLAM3::System::eSensor>(camera1type), use_viewer, 0, "Camera 1");//这行代码创建了一个名为SLAM1的ORB_SLAM3系统。它接受6个参数
/**
argv[1]：第一个参数是文件路径，表示ORB_SLAM3的配置文件路径。
argv[3]：第三个参数是摄像机参数文件的路径，用于初始化相机模型。
static_cast<ORB_SLAM3::System::eSensor>(camera1type)：第四个参数是摄像机类型，使用static_cast将camera1type转换为ORB_SLAM3::System::eSensor枚举类型。
use_viewer：第五个参数表示是否使用视图器（viewer）。
0：第六个参数是相机的ID，这里设置为0。
"Camera 1"：第七个参数是相机的名称，这里设置为"Camera 1"。
**/
ORB_SLAM3::System SLAM2(argv[1], argv[4], static_cast<ORB_SLAM3::System::eSensor>(camera2type), use_viewer, 0, "Camera 2");
if(mode == "calib"){
cout << "atlas are loaded!!" << endl;
cout << "start to calib..." << endl;
CalibC2C c2c(&SLAM1, &SLAM2);//创建一个名为c2c的CalibC2C对象，该对象用于执行相机到相机（C2C）标定
c2c.RunCalib();//调用c2c对象的RunCalib()方法，开始执行相机标定
cout << "calib finish, exit" << endl;
return 0;
}
CalibC2C::CalibC2C(System* src, System* dst)//数src和dst分别指向System类的对象
{
// get Atlas
SubSystem* ssrc = static_cast<SubSystem*>(src);
SubSystem* sdst = static_cast<SubSystem*>(dst);
auto srcAtlas = static_cast<SubAtlas*>(ssrc->getAtlas());//getAtlas()方法获取Atlas对象，并将其强制转换为SubAtlas类的对象srcAtlas和dstAtlas,这个定义是继承了Atlas
auto dstAtlas = static_cast<SubAtlas*>(sdst->getAtlas());
srcAtlas->setFirstCurrentMap();//也是自定义的函数
dstAtlas->setFirstCurrentMap();
srcKFs = srcAtlas->GetAllKeyFrames();//获取Atlas中所有的关键帧，并将其分别赋值给srcKFs和dstKFs。
dstKFs = dstAtlas->GetAllKeyFrames();
mpKeyFrameDB = static_cast<SubKeyFrameDB*>(sdst->getKeyFrameDatabase());//KeyFrameDB对象，并将其强制转换为SubKeyFrameDB类的对象mpKeyFrameDB，当中存放有构建关键帧数据库，和词袋相关。
matcherBoW = new ORBmatcher(0.9, true);//回环
matcher = new ORBmatcher(0.75, true);//匹配
mpLC = new SubLoopClosing();
}
class SubKeyFrameDB : public KeyFrameDatabase
{
public:
using InvertedFileType = vector<list<KeyFrame*>>;
SubKeyFrameDB() = default;
InvertedFileType& getInvertedFile() { return mvInvertedFile; }
const ORBVocabulary* getVocabulary() { return mpVoc; }
};
class SubLoopClosing : public LoopClosing
{
public:
SubLoopClosing() : LoopClosing(nullptr, nullptr, nullptr, true, true) {}
bool SubDetectCommonRegionsFromLastKF(KeyFrame* pCurrentKF, KeyFrame* pMatchedKF, g2o::Sim3 &gScw, int &nNumProjMatches,
std::vector<MapPoint*> &vpMPs, std::vector<MapPoint*> &vpMatchedMPs)
{
return DetectCommonRegionsFromLastKF(pCurrentKF, pMatchedKF, gScw, nNumProjMatches, vpMPs, vpMatchedMPs);
}//判断当前关键帧是否可以有效匹配
};
class SubSystem : public System
{
public:
SubSystem(const string &strVocFile, const string &strSettingsFile, const eSensor sensor, const bool bUseViewer = true, const int initFr = 0, const string &strSequence = std::string())
: System(strVocFile, strSettingsFile, sensor, bUseViewer, initFr, strSequence) {}
KeyFrameDatabase* getKeyFrameDatabase() { return mpKeyFrameDatabase; }
Atlas* getAtlas() { return mpAtlas; }
};
class SubAtlas : public Atlas
{
public:
void setFirstCurrentMap(){
mpCurrentMap = *(mspMaps.begin());
mpCurrentMap->SetCurrentMap();
}
};
2.2 图像序列捕获与地图创建(ORB-SLAM3操作)
在纹理丰富的场景中执行SLAM之前，应预先校准内参数，这将深刻影响SLAM和标定的准确性。在这种方法中，地图坐标系与第一幅图像的相机坐标系重合，因此相机可以以不同的频率进行拍摄，如果第一幅图像中的特征点很少，ORB-SLAM3系统将拒绝创建关键帧，因此，建议在开始时将相机放置在具有相对丰富特征的方向上。
在外参标定过程中，相机可能不会同步。因此，如果相机在SLAM开始时移动，假设t_0和t_1分别是两个相机捕获第一幅图像的时刻，则外参数T^{A_1}_{B_1}将产生一个误差T^{B_{t_0}}_{B_{t_1}}：
其中，T^{A_{t_0}}_{B_{t_1}}是实际的外参数，而T^{B_{t_0}}_{B_{t_1}}是相机B从时刻t_1到时刻t_0的变换。因此，如果相机B 移动速度较快，并且以较低的频率捕获图像，误差将更大。因此，建议在开始时保持相机静止,使T^{B_{t_0}}_{B_{t_1}}成为单位矩阵，从而避免理论误差。
从理论上讲如果相机在SLAM结束时保持静止，T^{A_n}_{B_m}也将等同于外参数，但在SLAM系统中，姿态估计是一个递归过程，累积误差会逐渐增加，如图3所示。因此，在完成标定时我们可以通过将T^{A_1}_{B_1}与T^{A_n}_{B_m}进行比较来判断标定是否成功。
图3. 累积误差对标定结果的影响，由于SLAM的漂移SLAM结束时会出现累积误差，因此T^{A_1}_{B_1}是比T^{A_n}_{B_m}更准确的相机外参估计结果。
降低误差的有效方法是回环检测，回环检测确定机器人是否返回到先前经过的位置。如果检测到回环，它将优化所有SLAM姿态和地图点以提高准确性。此外，ORB-SLAM3根据关键帧之间的连接关系优化局部地图，因此如果它们之间有重叠的视角，则累积误差将较小。综上所述，提出了三条操作规则：
1）在SLAM的开始和结束时，机器应保持静止。
2）如果相机之间有重叠的视野，建议将机器移动一小段距离，但不要转动过多，例如图7(a)中的运动轨迹。
3）否则，最好使机器在环形轨迹中移动，以便系统检测到回环闭合，例如图8(b)中的运动轨迹。
为了更容易检测回环，我们修改了ORB-SLAM3的回环检测模块，例如增加了关键帧的数量。总体上，这些操作规则不仅旨在使机器捕获足够相似的图像，而且还要保持累积误差较小。值得注意的是，如果相机中有一个是单目相机，则必须移动机器，而不仅仅是旋转，否则单目SLAM无法估计此情况下的姿态。由于相机的精度会随着距离的变化而改变，我们移除比相机基线深10倍的地图点，在SLAM结束时，进行全局捆绑调整（BA）以优化地图点的位置和关键帧的姿态。
vector<KeyFrame *> CalibC2C::DetectNBestCandidates(KeyFrame *pKF, int N) {
vector<KeyFrame *>
res; //创建一个空的vector<KeyFrame*>类型的变量res，用于存储共享词汇的关键帧
// mpKeyFrameBD 关键帧数据库的倒排文件和词汇表
auto invertedFile = mpKeyFrameDB->getInvertedFile();
auto orbvoc = mpKeyFrameDB->getVocabulary();
list<KeyFrame *> lKFsSharingWords; //用于存储共享词汇的关键帧
//遍历当前关键帧pKF的词袋表示
for (DBoW2::BowVector::const_iterator vit = pKF->mBowVec.begin(),
vend = pKF->mBowVec.end();
vit != vend; vit++) {
list<KeyFrame *> &lKFs = invertedFile
[vit->first]; //对于每个词汇，获取倒排文件中与之对应的关键帧列表
for (list<KeyFrame *>::iterator lit = lKFs.begin(), lend = lKFs.end();
lit != lend; lit++) {
KeyFrame *pKFi = *lit; //指针传入，即可以直接完成修改
// not place rec query before
if (pKFi->mnPlaceRecognitionQuery !=
pKF->mnId) //对于每个关键帧pKFi，判断其mnPlaceRecognitionQuery是否等于当前关键帧pKF的mnId，以防止重复查询。
{
//表示该关键帧还未进行过位置识别查询
pKFi->mnPlaceRecognitionWords = 0;
pKFi->mnPlaceRecognitionQuery = pKF->mnId;
lKFsSharingWords.push_back(pKFi);
}
pKFi->mnPlaceRecognitionWords++;
}
}
if (lKFsSharingWords.empty())
return res; //判断是否找到共享词汇的关键帧，如果没有找到则返回re
cout << "share same words kf size: " << lKFsSharingWords.size() << endl;
// find threshold and filter them
auto it = std::max_element(
lKFsSharingWords.begin(), lKFsSharingWords.end(),
[](KeyFrame *a, KeyFrame *b) {
return a->mnPlaceRecognitionWords < b->mnPlaceRecognitionWords;
}); //根据先后次序完成排序，找到共享词汇最多的关键帧
int minCommonWords = (*it)->mnPlaceRecognitionWords *
0.5f; //根据共享词汇数量的阈值对共享词汇的关键帧进行过滤
// 用于存储关键帧的得分和匹配
list<pair<float, KeyFrame *>> lScoreAndMatch;
for (list<KeyFrame *>::iterator lit = lKFsSharingWords.begin(),
lend = lKFsSharingWords.end();
lit != lend; lit++) {
KeyFrame *pKFi = *lit;
if (pKFi->mnPlaceRecognitionWords > minCommonWords) {
float si = orbvoc->score(
pKF->mBowVec,
pKFi->mBowVec); //如果其共享词汇数量大于阈值，则计算其与当前关键帧pKF之间的得分
pKFi->mPlaceRecognitionScore = si;
lScoreAndMatch.push_back(
make_pair(si, pKFi)); //并将其得分和关键帧添加到lScoreAndMatch中
}
}
if (lScoreAndMatch.empty())
return res;
//输出过滤后的共享词汇关键帧数量和阈值。
cout << "after filter words less than " << minCommonWords << endl
<< "size: " << lScoreAndMatch.size() << endl;
// 用于存储关键帧的累计得分和匹配
list<pair<float, KeyFrame *>> lAccScoreAndMatch;
float bestAccScore = 0;
for (auto it = lScoreAndMatch.begin(), itend = lScoreAndMatch.end();
it != itend; it++) {
KeyFrame *pKFi = it->second;
vector<KeyFrame *> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(
10); //对于每个得分和匹配对，获取关键帧的最佳共视关键帧列表，这个就是根据词袋计算得分获得的
float bestScore = it->first; //初始化最佳得分为当前得分
float accScore = bestScore;
KeyFrame *pBestKF = pKFi;
for (auto vit = vpNeighs.begin(), vend = vpNeighs.end(); vit != vend;
vit++) {
KeyFrame *pKF2 = *vit;
if (pKF2->mnPlaceRecognitionQuery ==
pKF->mnId) //对于每个共视关键帧pKF2，如果其mnPlaceRecognitionQuery等于当前关键帧pKF的mnId，则更新累计得分，并找到累计得分最高的关键帧。
{
accScore += pKF2->mPlaceRecognitionScore;
// find best score kf in covis kfs of this kf
if (pKF2->mPlaceRecognitionScore > bestScore) {
pBestKF = pKF2;
bestScore = pKF2->mPlaceRecognitionScore;
}
}
}
lAccScoreAndMatch.push_back(make_pair(accScore, pBestKF));
if (accScore > bestAccScore)
bestAccScore = accScore; //累计得分和最佳关键帧添加到lAccScoreAndMatch
}
cout << "find best covis group size: " << lAccScoreAndMatch.size() << endl;
lAccScoreAndMatch.sort(
[](const pair<float, KeyFrame *> &a, const pair<float, KeyFrame *> &b) {
return a.first > b.first;
}); //根据累计得分对lAccScoreAndMatch进行排序
// 创建一个空的set<KeyFrame*>类型的变量duplicatedKF，用于存储已经添加到lAccScoreAndMatch中的关键帧
set<KeyFrame *> duplicatedKF;
for (auto it = lAccScoreAndMatch.begin(); it != lAccScoreAndMatch.end();
it++) {
KeyFrame *pKFi = it->second;
if (pKFi->isBad())
continue; //每个累计得分和关键帧对，如果关键帧未被标记为坏关键帧
if (duplicatedKF.count(pKFi) == 0) { //未在duplicatedKF中出现过
duplicatedKF.insert(pKFi);
res.push_back(pKFi);
if (res.size() >= N)
break;
}
}
return res;
}
2.3 相似关键帧检测
与回环检测类似，需要找到所有相似关键帧对(A_i, B_j)对应的(i, j) ∈ M，以及两个地图中的匹配地图点P_k^{A_1}，P_k^{B_1}，我们使用词袋（BoW）模块来找到匹配的关键帧和地图点，该模块可以将特征点转换为向量，因此我们可以通过比较向量之间的距离来找到相似的图像。然后可以通过它们的ORB描述子来匹配地图点， 用于验证匹配关系，我们可以通过以下公式获得关键帧A_i和B_j之间的相似变换S^{A_i}_{B_j}：
其中，R是一个正交旋转矩阵，描述了两个参考帧之间的相对方向关系，t是两个参考帧之间的平移向量，λ代表两个坐标系之间的尺度因子，当所有相机都是立体相机或RGB-D相机时，λ = 1。（SIM3）相似变换S_{B_j}^{A_i}可以通过式(3)中的双向重投影方法进行优化。
如图4所示，它将匹配地图点P_k^{A_i}、P_k^{B_j}一起投影到关键帧A_i、B_j上的像素点，像素误差分别在A_i和B_j中由马氏距离来描述，如果相机具有不同的视场和分辨率，则重投影误差的权重将不相同。带有协方差的马氏距离比欧氏距离更准确地描述了误差
ρ(·)是鲁棒核函数，用于减少异常值的影响，使用卡方检验来消除异常值，如果正确匹配的地图点数超过阈值，我们认为相似的关键帧匹配成功，这是“帧对帧”对齐，也是第一阶段的优化，在非线性优化问题中，初始值非常重要，所以我们选择T^{A_1}_{B_1}作为第二阶段优化的初始值：
其中，S^{A_i}_{B_j} 是通过大多数匹配地图点计算得出的。
// 该函数的作用是从候选的关键帧中检测共同的地图点，并求解相似变换（Sim3）
bool CalibC2C::DetectCommonRegionsFromCand(
KeyFrame *pKF, vector<KeyFrame *> &vpCand, KeyFrame *&pMatchedKF2,
g2o::Sim3 &g2oScw, int &bestMatchesReprojNum, vector<MapPoint *> &vpMPs,
vector<MapPoint *> &vpMatchedMPs) {
//输入参数包括一个当前关键帧pKF，一个候选关键帧的集合vpCand
KeyFrame *pBestMatchedKF; //最佳匹配关键帧
int nBestMatchesReproj = 0; //重投影匹配数量
int nBestNumCoindicendes = 0; //共同地图点数量
g2o::Sim3 g2oBestScw; //最佳相似变换
vector<MapPoint *> vpBestMapPoints; //最佳匹配的地图点集合
vector<MapPoint *> vpBestMatchedMapPoints;
for (KeyFrame *pKFi : vpCand) {
if (!pKFi || pKFi->isBad())
continue;
// 1. match pKF with cov kf of pKFi in Cand, find the best one
// searchByBoW, brute-force search from two
// kf,首先使用词袋模型进行匹配，找到与当前关键帧pKF具有最多匹配的关键帧
auto vpCovKFi =
pKFi->GetBestCovisibilityKeyFrames(5); //获取pKFi的最佳共视关键帧列表
vector<vector<MapPoint *>> vvpMatchedMPs(
vpCovKFi.size()); //存储匹配的地图点
int nMostBoWNumMatches = 0;
KeyFrame *pMostBoWMatchesKF = nullptr;
for (int j = 0; j < vpCovKFi.size(); ++j) { //遍历pKFi的最佳共视关键帧列表
if (!vpCovKFi[j] || vpCovKFi[j]->isBad())
continue;
int num = matcherBoW->SearchByBoW(
pKF, vpCovKFi[j], vvpMatchedMPs[j]); //使用词袋模型进行匹配
if (num > nMostBoWNumMatches) { //找到与当前关键帧pKF具有最多匹配的关键帧
nMostBoWNumMatches = num;
pMostBoWMatchesKF = vpCovKFi[j];
}
}
// 2. 找到所有属于pKFi及其共视关系和pKF本身的mp,并创建optimizeSim3接口
// 找到与当前关键帧pKF和pMostBoWMatchesKF共同观测的地图点，并保存它们的匹配关键帧
auto nMPNums = pKF->GetMapPointMatches().size(); //当前关键帧pKF的地图点数量
set<MapPoint *> duplicateMP;
vector<MapPoint *> vpMatchedPoints(nMPNums, nullptr); // mp
vector<KeyFrame *> vpKeyFrameMatchedMP(nMPNums, nullptr); // mp to kf
int numBoWMatches = 0;
for (int j = 0; j < vpCovKFi.size(); j++) {
for (int k = 0; k < vvpMatchedMPs[j].size();
k++) { //遍历与pKF和pMostBoWMatchesKF共同观测的地图点
MapPoint *mp = vvpMatchedMPs[j][k]; //获取匹配点
if (!mp || mp->isBad())
continue;
if (duplicateMP.count(mp) == 0) {
duplicateMP.insert(mp);
numBoWMatches++;
vpMatchedPoints[k] = mp; //保存匹配点
vpKeyFrameMatchedMP[k] = vpCovKFi[j]; //保存匹配点对应的关键帧
}
}
}
cout << "search by bow get matched mp size: " << numBoWMatches << endl;
// 3. 共同观测的地图点数量大于等于20个，则使用Sim3求解相似变换
if (numBoWMatches >= 20) {
bool bFixedScale = false;
int nBoWInliers = 8;
Sim3Solver solver(pKF, pMostBoWMatchesKF, vpMatchedPoints, bFixedScale,
vpKeyFrameMatchedMP);
solver.SetRansacParameters(0.99, nBoWInliers, 300); //设置RANSAC算法的参数
bool bNoMore = false;
vector<bool> vbInliers;
int nInliers;
bool bConverge = false;
cv::Mat mTcm;
while (!bConverge && !bNoMore) {
mTcm = Converter::toCvMat(solver.iterate(
20, bNoMore, vbInliers, nInliers,
bConverge)); //然后通过迭代求解相似变换，直到收敛或达到最大迭代次数。
}
if (bConverge) {
cout << "solve sim3 converged!!" << endl;
// 4.
// 如果成功求解相似变换，则准备从pMostBoWMatchesKF的共视关键帧中搜索更多的匹配地图点。
vpCovKFi.clear();
vpCovKFi = pMostBoWMatchesKF->GetBestCovisibilityKeyFrames(
5); //获取pMostBoWMatchesKF的最佳共视关键帧，并保存它们的匹配地图点
duplicateMP.clear();
vector<MapPoint *> vpMapPoints;
vector<KeyFrame *> vpKeyFrames;
for (KeyFrame *pCovKFi :
vpCovKFi) { //遍历pMostBoWMatchesKF的最佳共视关键帧
for (
MapPoint *mp :
pCovKFi
->GetMapPointMatches()) { //遍历pMostBoWMatchesKF的最佳共视关键帧的地图点
if (!mp || mp->isBad())
continue;
if (duplicateMP.count(mp) == 0) {
duplicateMP.insert(mp);
vpMapPoints.push_back(mp);
vpKeyFrames.push_back(pCovKFi);
}
}
}
g2o::Sim3 gScm(solver.GetEstimatedRotation().cast<double>(),
solver.GetEstimatedTranslation().cast<double>(),
solver.GetEstimatedScale()); //获取相似变换
g2o::Sim3 gSmw(pMostBoWMatchesKF->GetRotation().cast<double>(),
pMostBoWMatchesKF->GetTranslation().cast<double>(),
1.0); //获取pMostBoWMatchesKF的相似变换
g2o::Sim3 gScw = gScm * gSmw; // 当前帧相对于世界坐标系的相似变换
Sophus::Sim3<float> mScw =
Converter::toSophus(gScw); //转换为Sophus::Sim3<float>类型
vpMatchedMPs.assign(nMPNums, static_cast<MapPoint *>(nullptr));
vpKeyFrameMatchedMP.assign(nMPNums, static_cast<KeyFrame *>(nullptr));
int numProjMatches = matcher->SearchByProjection(
pKF, mScw, vpMapPoints, vpKeyFrames, vpMatchedMPs,
vpKeyFrameMatchedMP, 8,
1.5); //投影法（searchByProjection）从pKF的共视关键帧中搜索匹配地图点
cout << "search by projection get size: " << numProjMatches << endl;
// 5.
// 搜索到的匹配地图点数量大于等于25个，则使用优化方法（OptimizeSim3）优化相似变换
if (numProjMatches >= 25) {
Eigen::Matrix<double, 7, 7> mHessian7x7;
int numOptMatches =
Optimizer::OptimizeSim3(pKF, pKFi, vpMatchedMPs, gScm, 10,
bFixedScale, mHessian7x7, true);
cout << "optim sim3 get size: " << numOptMatches << endl;
// 6. when optimed Scm is found, search by projection again to get
// more matched points
// 如果优化成功，则根据优化后的相似变换再次使用投影法搜索匹配地图点，并保存它们的匹配关键帧
if (numOptMatches >= 10) {
gScw = gScm * gSmw;
vpMatchedMPs.assign(nMPNums, static_cast<MapPoint *>(nullptr));
// shrink search range
numProjMatches = matcher->SearchByProjection(
pKF, mScw, vpMapPoints, vpMatchedMPs, 5,
2.0); //投影法（searchByProjection）从pKF的共视关键帧中搜索匹配地图点
cout << "after optim sim3 search by projection get size: "
<< numProjMatches << endl;
// 如果搜索到的匹配地图点数量大于等于40个，则进行几何验证
if (numProjMatches >= 40) {
// 7. geometry validation: use covis kf of pKF to detect common
// regions of MostBoWMatchKF
vector<KeyFrame *> vpCurrentCovKFs =
pKF->GetBestCovisibilityKeyFrames(5);
int validKF = 0;
for (KeyFrame *pKFj : vpCurrentCovKFs) { // pKFj共视关键帧
Eigen::Matrix4d mTjc =
(pKFj->GetPose() * pKF->GetPoseInverse())
.matrix()
.cast<
double>(); // pKFj相对于pKF的变换矩阵,计算它们之间的相似变换
g2o::Sim3 gSjc(mTjc.topLeftCorner<3, 3>(),
mTjc.topRightCorner<3, 1>(),
1.0); //转换为g2o::Sim3类型
g2o::Sim3 gSjw = gSjc * gScw;
int numProjMatches_j = 0;
vector<MapPoint *> vpMatchedMPs_j;
bool bValid = mpLC->SubDetectCommonRegionsFromLastKF(
pKFj, pMostBoWMatchesKF, gSjw, numProjMatches_j,
vpMapPoints,
vpMatchedMPs_j); //使用局部地图一致性（mpLC）进行几何验证
if (bValid) //如果通过几何验证的关键帧数量大于等于1个，则保存当前结果为最佳匹配，并更新最佳匹配的相关参数
validKF++;
}
cout << "geometry validation pass kf size: " << validKF << endl;
// 8. save best result along the iteration
if (nBestMatchesReproj < numProjMatches) {
nBestMatchesReproj = numProjMatches;
nBestNumCoindicendes = validKF;
pBestMatchedKF = pMostBoWMatchesKF;
g2oBestScw = gScw;
vpBestMapPoints = vpMapPoints;
vpBestMatchedMapPoints = vpMatchedMPs;
}
}
}
}
}
}
}
//遍历完所有候选关键帧后，如果最佳匹配的重投影匹配数量大于0，则将最佳匹配的结果赋值给输出参数
if (nBestMatchesReproj) {
bestMatchesReprojNum = nBestMatchesReproj;
pMatchedKF2 = pBestMatchedKF;
g2oScw = g2oBestScw;
vpMPs = vpBestMapPoints;
vpMatchedMPs = vpBestMatchedMapPoints;
return nBestNumCoindicendes >= 1;
}
return false;
}
2.4 外参数优化
对于第二阶段的优化，将所有匹配地图点（P_k^{A_i}，P_k^{B_j}）转换为（P_k^{A_1}，P_k^{B_1}）。然后，通过双向重投影，将外参标定转换为非线性优化问题：
经过几次非线性优化迭代和卡方检验去除异常值，可以估计出外部参数 T_{B_1}^{A_1} 以及通过卡方检验的匹配地图点的数量，最后可以得到变换矩阵T_{B_m}^{A_n}。“地图对齐”信息的优化方法比“帧对齐”信息的方法更好。影响校准精度的主要因素是SLAM系统中的漂移，因此我们从以下两个方面判断校准是否成功。一方面，T_{B_1}^{A_1}和T_{B_m}^{A_n}在理论上都是外参数；由于SLAM漂移的影响，T_{B_m}^{A_n}与实际值有一定偏差。因此，建议在T_{B_1}^{A_1} 和 T_{B_m}^{A_n}之间存在较大差异时重新进行校准。另一方面，如果只有少数地图点通过卡方检验，结果与“帧对齐”校准相同，这样的结果不够精确。特别地，对于相机A是立体或RGB-D相机，相机B是单目相机的情况，T_{B_1}^{A_1}是带有尺度因子λ的相似变换，如下所示
由于旋转矩阵R_{B_1}^{A_1}的正交性，可以轻松得到实际的外部参数R_{B_1}^{A_1}和t_{B_1}^{A_1}。对于两个相机都是单目相机的情况，考虑到在单目SLAM的过程中无法获取真实的尺度信息，我们只能计算3个自由度的旋转R_{B_1}^{A_1}。
int CalibC2C::OptimizeSim3ForCalibr(const vector<KeyFrame *> &vpKF1s,
const vector<KeyFrame *> &vpKF2s,
vector<vector<MapPoint *>> &vvpMatches,
g2o::Sim3 &g2oS12, const float th2,
const bool bFixScale,
Eigen::Isometry3f finalPose1,
Eigen::Isometry3f finalPose2) {
// 1. 初始化g2o优化器
g2o::SparseOptimizer optimizer;
g2o::BlockSolverX::LinearSolverType *linearSolver;
linearSolver =
new g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>();
g2o::BlockSolverX *solver_ptr = new g2o::BlockSolverX(linearSolver);
g2o::OptimizationAlgorithmLevenberg *solver =
new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
optimizer.setAlgorithm(solver);
// optimizer.setVerbose(true);
const float deltaHuber = sqrt(th2);
// 设置相机内参数：从输入的关键帧中获取相机内参数，然后将其设置到g2o优化器的顶点中。
KeyFrame *KF1 = vpKF1s.front();
KeyFrame *KF2 = vpKF2s.front();
// 2. set vertex of extrinsic
g2o::VertexSim3Expmap *vSim3 = new g2o::VertexSim3Expmap();
vSim3->_fix_scale = bFixScale;
vSim3->setFixed(false);
vSim3->_principle_point1[0] = KF1->cx;
vSim3->_principle_point1[1] = KF1->cy;
vSim3->_focal_length1[0] = KF1->fx;
vSim3->_focal_length1[1] = KF1->fy;
vSim3->_principle_point2[0] = KF2->cx;
vSim3->_principle_point2[1] = KF2->cy;
vSim3->_focal_length2[0] = KF2->fx;
vSim3->_focal_length2[1] = KF2->fy;
vSim3->setId(0);
optimizer.addVertex(vSim3);
// 3. 设置地图点和边：遍历输入的关键帧，为每个关键帧中的匹配地图点设置顶点和边，并将边添加到优化器中。
int nKF = vpKF1s.size();
int nCorrespondences = 0;
int id1 = -1;
int id2 = 0;
vector<vector<g2o::EdgeSim3ProjectXYZForCalibr *>> vvpEdges12(nKF);
vector<vector<g2o::EdgeInverseSim3ProjectXYZForCalibr *>> vvpEdges21(nKF);
for (int idx = 0; idx < nKF; idx++) {//遍历所有关键帧，对每个关键帧的匹配地图点进行处理
KeyFrame *pKF1 = vpKF1s[idx];
KeyFrame *pKF2 = vpKF2s[idx];
const vector<MapPoint *> &vpMapPoints = pKF1->GetMapPointMatches();
const vector<MapPoint *> &vpMatches = vvpMatches[idx];
int N = vpMatches.size();
vector<g2o::EdgeSim3ProjectXYZForCalibr *> vpEdges12;
vector<g2o::EdgeInverseSim3ProjectXYZForCalibr *> vpEdges21;
vpEdges12.reserve(N);
vpEdges21.reserve(N);
Eigen::Matrix3d R1w, R2w;
Eigen::Vector3d t1w, t2w;
Eigen::Isometry3f P1w, P2w;
P1w.matrix() = pKF1->GetPose().matrix() * finalPose1.matrix();
P2w.matrix() = pKF2->GetPose().matrix() * finalPose2.matrix();//首先获取关键帧的位姿，并将其与初始位姿相乘得到最终位姿
R1w = P1w.rotation().cast<double>();
R2w = P2w.rotation().cast<double>();
t1w = P1w.translation().cast<double>();
t2w = P2w.translation().cast<double>();
for (int i = 0; i < N; i++) {
if (!vpMatches[i])
continue;
auto pMP1 = vpMapPoints[i];
auto pMP2 = vpMatches[i];
const int i2 = get<0>(pMP2->GetIndexInKeyFrame(pKF2));
if (pMP1 == nullptr || pMP2 == nullptr || pMP1->isBad() ||
pMP2->isBad() || i2 < 0)
continue;
id1 += 2;
id2 += 2;
nCorrespondences++;
// mp see from camera 1
g2o::VertexSBAPointXYZ *vPoint1 = new g2o::VertexSBAPointXYZ();//地图点分别添加为顶点
Eigen::Vector3f P3D1w = pMP1->GetWorldPos();
// trans to final if need
P3D1w = finalPose1.inverse() * P3D1w;
vPoint1->setEstimate(P3D1w.cast<double>());
vPoint1->setId(id1);
vPoint1->setFixed(true);
optimizer.addVertex(vPoint1);
// mp see from camera 2
g2o::VertexSBAPointXYZ *vPoint2 = new g2o::VertexSBAPointXYZ();
Eigen::Vector3f P3D2w = pMP2->GetWorldPos();
P3D2w = finalPose2.inverse() * P3D2w;
vPoint2->setEstimate(P3D2w.cast<double>());
vPoint2->setId(id2);
vPoint2->setFixed(true);
optimizer.addVertex(vPoint2);
// link edge
// 1. mp from 2 project to 1 and make residual with mp from 1
Eigen::Matrix<double, 2, 1> obs1;
const cv::KeyPoint &kpUn1 = pKF1->mvKeysUn[i];
obs1 << kpUn1.pt.x, kpUn1.pt.y;
g2o::EdgeSim3ProjectXYZForCalibr *e12 =
new g2o::EdgeSim3ProjectXYZForCalibr(R1w, t1w);//并创建两个边，分别连接地图点和两个相机
e12->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(
optimizer.vertex(id2)));
e12->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(
optimizer.vertex(0)));
e12->setMeasurement(obs1);
const float &invSigmaSquare1 = pKF1->mvInvLevelSigma2[kpUn1.octave];
e12->setInformation(Eigen::Matrix2d::Identity() * invSigmaSquare1);//边的测量值为关键点在图像上的坐标，信息矩阵为关键点的尺度信息
g2o::RobustKernelHuber *rk1 = new g2o::RobustKernelHuber;
e12->setRobustKernel(rk1);
rk1->setDelta(deltaHuber);
optimizer.addEdge(e12);
// 2. mp from 1 project to 2 and make residual with mp from 2
Eigen::Matrix<double, 2, 1> obs2;
const cv::KeyPoint &kpUn2 = pKF2->mvKeysUn[i2];
obs2 << kpUn2.pt.x, kpUn2.pt.y;
g2o::EdgeInverseSim3ProjectXYZForCalibr *e21 =
new g2o::EdgeInverseSim3ProjectXYZForCalibr(R2w, t2w);
e21->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(
optimizer.vertex(id1)));
e21->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(
optimizer.vertex(0)));
e21->setMeasurement(obs2);
float invSigmaSquare2 = pKF2->mvInvLevelSigma2[kpUn2.octave];
e21->setInformation(Eigen::Matrix2d::Identity() * invSigmaSquare2);
g2o::RobustKernelHuber *rk2 = new g2o::RobustKernelHuber;
e21->setRobustKernel(rk2);
rk2->setDelta(deltaHuber);
optimizer.addEdge(e21);
vpEdges12.push_back(e12);
vpEdges21.push_back(e21);
}
vvpEdges12[idx] = vpEdges12;
vvpEdges21[idx] = vpEdges21;
}
// 4. 开始优化：进行多次迭代优化，并根据残差大小将边标记为inlier或outlier。
int outliers;
for (int i = 0; i < 4; i++) {
vSim3->setEstimate(g2oS12);//迭代优化4次，每次将Sim3顶点设置为初始值
optimizer.initializeOptimization(0);
optimizer.optimize(10);
outliers = 0;
for (int j = 0; j < nKF; j++) {
auto &vpEdges12 = vvpEdges12[j];
auto &vpEdges21 = vvpEdges21[j];
for (int k = 0; k < vpEdges12.size(); k++) {
auto e12 = vpEdges12[k];
auto e21 = vpEdges21[k];
if (!e12 || !e21)
continue;
if (e12->chi2() > th2 || e21->chi2() > th2) {
e12->setLevel(1);
e21->setLevel(1);//根据边的残差大小判断是否为离群值，将离群值的边设置为Level 1，再次进行优化
outliers++;
} else {
e12->setLevel(0);
e21->setLevel(0);
}
}
}
}
// 计算重投影误差：遍历所有边，计算inlier边的重投影误差，并将其累加。
double SumChi2 = 0;
for (int j = 0; j < nKF; j++) {
auto &vpEdges12 = vvpEdges12[j];
auto &vpEdges21 = vvpEdges21[j];
for (int k = 0; k < vpEdges12.size(); k++) {
auto e12 = vpEdges12[k];
auto e21 = vpEdges21[k];
if (!e12 || !e21)
continue;
if (e12->chi2() <= th2 && e21->chi2() <= th2)
SumChi2 += sqrt(abs(e12->chi2())) + sqrt(abs(e21->chi2()));//将残差小于阈值的边的残差平方根相加，最后除以边的数量得到平均重投影误差
}
}
SumChi2 /= (2 * (nCorrespondences - outliers));
g2o::VertexSim3Expmap *vSim3_recov =
static_cast<g2o::VertexSim3Expmap *>(optimizer.vertex(0));
g2oS12 = vSim3_recov->estimate();
return nCorrespondences - outliers;
}
评论（0）
您还未登录，请登录后发表或查看评论