# Initializer.cpp
### Initializer::Initialize()
用RANSAC方法[知識點1]平行計算基礎矩陣(F)[知識點2]和單應性矩陣(H)[知識點3]，選取其中一個模型，恢復出最開始兩幀之間的相對姿態。
在[Tracking::MonocularInitialization()](#Tracking::MonocularInitialization())中引用
#### 定義
```
bool Initializer::Initialize(const Frame &CurrentFrame, //輸入當前幀
const vector<int> &vMatches12, //當前幀與參考幀的匹配關係
cv::Mat &R21, //
cv::Mat &t21,
vector<cv::Point3f> &vP3D,
vector<bool> &vbTriangulated)
```
#### 函式流程
1. 重新紀錄特徵點匹配關係
2. 生成匹配特徵點索引
3. 在所有匹配特徵點對中隨機選擇8對匹配特徵點為一組，每組都能用來估計H與F矩陣
4. 用兩個線程分別計算F與H矩陣
#### 知識點
#### 函式詳解
1. 重新紀錄特徵點匹配關係
```
//當前幀的特徵點
mvKeys2 = CurrentFrame.mvKeysUn;
// mvMatches12記錄匹配上的特徵點對
mvMatches12.clear();
mvMatches12.reserve(mvKeys2.size());
// 記錄每個特徵點是否有匹配的特徵點，
// 這個變數後面沒有用到，後面只關心匹配上的特徵點
mvbMatched1.resize(mvKeys1.size());
//遍歷所有匹配點對
for(size_t i=0, iend=vMatches12.size();i<iend; i++)
{
//是否有匹配關係
if(vMatches12[i]>=0)
{
//儲存匹配點對的index
mvMatches12.push_back(make_pair(i,vMatches12[i]));
mvbMatched1[i]=true;
}
else
mvbMatched1[i]=false;
}
```
2. 生成匹配特徵點索引
```
// 有匹配的特徵點的對數
const int N = mvMatches12.size();
// Indices for minimum set selection
// 儲存特徵點索引
vector<size_t> vAllIndices;
vAllIndices.reserve(N);
vector<size_t> vAvailableIndices;
//生成0到N-1的數作為匹配點對的索引
for(int i=0; i<N; i++)
{
vAllIndices.push_back(i);
}
```
3. 在所有匹配特徵點對中隨機選擇8對匹配特徵點為一組，每組都能用來估計H與F矩陣
```
// Generate sets of 8 points for each RANSAC iteration
//儲存每次迭代所使用的向量
mvSets = vector< vector<size_t> >(mMaxIterations, //坐大迭代次數
vector<size_t>(8,0)); //初始值為0長度為8的向量
//設定隨機變數
DUtils::Random::SeedRandOnce(0);
//開始迭代
for(int it=0; it<mMaxIterations; it++)
{
// 可用匹配點對
vAvailableIndices = vAllIndices;
// Select a minimum set
//遍歷8個點對
for(size_t j=0; j<8; j++)
{
// 產生0到N-1的隨機數
int randi = DUtils::Random::RandomInt(0,vAvailableIndices.size()-1);
// idx表示哪一個索引對應的匹配點對被選中
int idx = vAvailableIndices[randi];
mvSets[it][j] = idx;
// randi對應的索引已經被選過了，從容器中刪除
// randi對應的索引用最後一個元素替換，並刪掉最後一個元素
vAvailableIndices[randi] = vAvailableIndices.back();
vAvailableIndices.pop_back();
}
}
```
4. 用兩個線程分別計算F與H矩陣
```
// Launch threads to compute in parallel a fundamental matrix and a homography
// 步驟3：調用多線程分別用於計算fundamental matrix和homography
vector<bool> vbMatchesInliersH, vbMatchesInliersF;
float SH, SF; // score for H and F
cv::Mat H, F; // H and F
// 構造線程
thread threadH(&Initializer::FindHomography, //線程主函式
this, //由於主函式為類的函式，因此指向當前對象
ref(vbMatchesInliersH), //輸出匹配點對的內點標記
ref(SH), //輸出H矩陣的RANSAC評分
ref(H)); //輸出H矩陣
// 計算fundamental matrix並打分
thread threadF(&Initializer::FindFundamental,
this,
ref(vbMatchesInliersF),
ref(SF),
ref(F));
// Wait until both threads have finished
//等待線程結束
threadH.join();
threadF.join();
```
Initializer::FindHomography : 跳至[Initializer::FindHomography()](#Initializer::FindHomography())
Initializer::FindFundamental : 跳至[Initializer::FindFundamental()](#Initializer::FindFundamental())
5. 選擇要用的矩陣來得到姿態
```
// Compute ratio of scores
// 步驟4：計算得分比例，選取某個模型
float RH = SH/(SH+SF);
// Try to reconstruct from homography or fundamental depending on the ratio (0.40-0.45)
if(RH>0.40)
return ReconstructH(vbMatchesInliersH,H,mK,R21,t21,vP3D,vbTriangulated,1.0,50); //用H矩陣計算姿態
else //if(pF_HF>0.6) 更傾向用基礎矩陣計算
return ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,1.0,50); //用F矩陣計算姿態
return false;
```
ReconstructH : 跳到[Initializer::ReconstructH()](#Initializer::ReconstructH())
ReconstructF : 跳到[Initializer::ReconstructF()](#Initializer::ReconstructF())
### Initializer::FindHomography()
計算單應矩陣(H)
在[Initializer::Initialize()](#Initializer::Initialize())中引用
#### 定義
```
void Initializer::FindHomography(vector<bool> &vbMatchesInliers, //標記是否為外點
float &score, //H矩陣的得分
cv::Mat &H21) //H矩陣
```
#### 函式流程
1. 座標歸一化[知識點1]
2. 選擇8個點對
3. 使用8點法計算H矩陣
4. 利用重投影誤差為當次RANSAC的結果評分
5. 更新最優單應矩陣，並保留對應的內外點標記
#### 知識點
1. 歸一化
在使用直接線性變換(DLT)算法求解基礎矩陣F或者單應矩陣H時，必須先進行數據歸一化處理。歸一化變換將消除由任意選取圖像坐標系的原點和尺度所產生的影響。
歸一化的方法包括圖像坐標的平移和尺度縮放，歸一化必須在實施DLT之前進行，然後再對結果進行適當的校正就能得到關於原坐標系的基礎矩陣F或者單應矩陣H。
歸一下流程:
* 對座標平移，使特徵點集合的中心移動至原點。
* 對點進行縮放，
#### 函式詳解
1. 座標歸一化
```
// Number of putative matches
//匹配的特徵點對數量
const int N = mvMatches12.size();
// Normalize coordinates
vector<cv::Point2f> vPn1, vPn2;
cv::Mat T1, T2;
// 將mvKeys1和mvKey2歸一化到均值為0，一階絕對矩為1(平均為1)，歸一化矩陣分別為T1、T2
Normalize(mvKeys1,vPn1, T1); //mvKeys1:參考幀的特徵點
Normalize(mvKeys2,vPn2, T2); //mvKeys2:當前幀的特徵點
cv::Mat T2inv = T2.inv();
```
跳到[Initializer::Normalize()](#Initializer::Normalize())
2. 選擇8個點對
```
// Best Results variables
// 最終最佳的MatchesInliers與得分
score = 0.0;
vbMatchesInliers = vector<bool>(N,false);
// Iteration variables
//參考幀的特徵點座標
vector<cv::Point2f> vPn1i(8);
//當前幀的特徵點座標
vector<cv::Point2f> vPn2i(8);
//單應矩陣，逆單應矩陣
cv::Mat H21i, H12i;
// 每次RANSAC的MatchesInliers與得分
vector<bool> vbCurrentInliers(N,false);
float currentScore;
// Perform all RANSAC iterations and save the solution with highest score
for(int it=0; it<mMaxIterations; it++)
{
// 這個應該最少4對匹配點就可以了
// Select a minimum set
for(size_t j=0; j<8; j++)
{
int idx = mvSets[it][j];
// vPn1i和vPn2i為匹配的特徵點對的坐標
vPn1i[j] = vPn1[mvMatches12[idx].first];
vPn2i[j] = vPn2[mvMatches12[idx].second];
}
```
3. 使用8點法計算H矩陣
```
cv::Mat Hn = ComputeH21(vPn1i,vPn2i);
// 恢復原始的均值和尺度
H21i = T2inv*Hn*T1;
//逆矩陣
H12i = H21i.inv();
```
跳到[Initializer::ComputeH21()](#Initializer::ComputeH21())
4. 利用重投影誤差為當次RANSAC的結果評分
```
currentScore = CheckHomography(H21i, H12i, vbCurrentInliers, mSigma);
```
跳到[Initializer::CheckHomography()](#Initializer::CheckHomography())
5. 更新最優單應矩陣，並保留對應的內外點標記
```
// 得到最優的vbMatchesInliers與score
if(currentScore>score)
{
H21 = H21i.clone();
vbMatchesInliers = vbCurrentInliers;
score = currentScore;
}
}
```
### Initializer::FindFundamental()
計算基礎矩陣(F)，流程與[Initializer::FindHomography()](#Initializer::FindHomography())相似
在[Initializer::Initialize()](#Initializer::Initialize())中引用
#### 定義
```
void Initializer::FindFundamental(vector<bool> &vbMatchesInliers, //標記是否為外點
float &score, //F矩陣的得分
cv::Mat &F21) //F矩陣
```
#### 函式流程
1. 座標歸一化
2. 選擇8個點對
3. 使用8點法計算F矩陣
4. 利用重投影誤差為當次RANSAC的結果評分
5. 更新最優單應矩陣，並保留對應的內外點標記
#### 函式詳解
1. 座標歸一化
H矩陣用逆矩陣，而F矩陣式用轉置矩陣
```
// Number of putative matches
const int N = vbMatchesInliers.size();
// Normalize coordinates
vector<cv::Point2f> vPn1, vPn2;
cv::Mat T1, T2;
Normalize(mvKeys1,vPn1, T1);
Normalize(mvKeys2,vPn2, T2);
//取轉置矩陣
cv::Mat T2t = T2.t();
```
2. 選擇8個點對
```
// Best Results variables
score = 0.0;
vbMatchesInliers = vector<bool>(N,false);
// Iteration variables
vector<cv::Point2f> vPn1i(8);
vector<cv::Point2f> vPn2i(8);
cv::Mat F21i;
vector<bool> vbCurrentInliers(N,false);
float currentScore;
// Perform all RANSAC iterations and save the solution with highest score
for(int it=0; it<mMaxIterations; it++)
{
// Select a minimum set
for(int j=0; j<8; j++)
{
int idx = mvSets[it][j];
vPn1i[j] = vPn1[mvMatches12[idx].first];
vPn2i[j] = vPn2[mvMatches12[idx].second];
}
```
3. 使用8點法計算F矩陣
```
cv::Mat Fn = ComputeF21(vPn1i,vPn2i);
F21i = T2t*Fn*T1;
```
跳至[Initializer::ComputeF21()](#Initializer::ComputeF21())
4. 利用重投影誤差為當次RANSAC的結果評分
```
// 利用重投影誤差為當次RANSAC的結果評分
currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma);
```
跳至[Initializer::CheckFundamental()](#Initializer::CheckFundamental())
5. 更新最優單應矩陣，並保留對應的內外點標記
```
if(currentScore>score)
{
F21 = F21i.clone();
vbMatchesInliers = vbCurrentInliers;
score = currentScore;
}
}
```
### Initializer::Normalize()
特徵歸一化
在[Initializer::FindHomography()](#Initializer::FindHomography())及[Initializer::FindFundamental](#Initializer::FindFundamental)中引用
#### 定義
```
void Initializer::Normalize(const vector<cv::KeyPoint> &vKeys, //輸入未歸一化的特徵點
vector<cv::Point2f> &vNormalizedPoints, //輸出歸一化後的特徵點
cv::Mat &T) //歸一化用的變換矩陣
```
#### 函式流程
1. 計算特徵點x,y座標的平均
2. 計算特徵點的偏移程度
3. 進行歸一化，使平均為1
4. 計算歸一化的轉換矩陣
#### 函式詳解
1. 計算特徵點x,y座標的平均
```
float meanX = 0;
float meanY = 0;
//特徵點數量
const int N = vKeys.size();
//儲存歸一下後的點對
vNormalizedPoints.resize(N);
//遍歷特徵點
for(int i=0; i<N; i++)
{
//數值累加
meanX += vKeys[i].pt.x;
meanY += vKeys[i].pt.y;
}
//取平均值
meanX = meanX/N;
meanY = meanY/N;
```
2. 計算特徵點的偏移程度
```
float meanDevX = 0;
float meanDevY = 0;
// 將所有vKeys點減去中心坐標，使x坐標和y坐標均值分別為0
for(int i=0; i<N; i++)
{
vNormalizedPoints[i].x = vKeys[i].pt.x - meanX;
vNormalizedPoints[i].y = vKeys[i].pt.y - meanY;
//累積偏移程度
meanDevX += fabs(vNormalizedPoints[i].x);
meanDevY += fabs(vNormalizedPoints[i].y);
}
//平均偏移程度
meanDevX = meanDevX/N;
meanDevY = meanDevY/N;
float sX = 1.0/meanDevX;
float sY = 1.0/meanDevY;
```
3. 進行歸一化，使平均為1
```
// 將x坐標和y坐標分別進行尺度縮放，使得x坐標和y坐標的一階絕對矩分別為1
for(int i=0; i<N; i++)
{
vNormalizedPoints[i].x = vNormalizedPoints[i].x * sX;
vNormalizedPoints[i].y = vNormalizedPoints[i].y * sY;
}
```
4. 計算歸一化的轉換矩陣
```
// |sX 0 -meanx*sX|
// |0 sY -meany*sY|
// |0 0 1 |
T = cv::Mat::eye(3,3,CV_32F);
T.at<float>(0,0) = sX;
T.at<float>(1,1) = sY;
T.at<float>(0,2) = -meanX*sX;
T.at<float>(1,2) = -meanY*sY;
```
### Initializer::ComputeH21()
用8點法計算H矩陣
在[Initializer::FindHomography()](#Initializer::FindHomography())中被引用
#### 定義
```
cv::Mat Initializer::ComputeH21(const vector<cv::Point2f> &vP1, //參考幀歸一化後的特徵點
const vector<cv::Point2f> &vP2) //當前幀歸一化後的特徵點
```
#### 函式流程
1. 構造矩陣A[知識點1]
2. SVD分解，求出單應矩陣
#### 知識點
1. 單應矩陣
特徵點p1經過單應矩陣H21轉換至p2:
矩陣形式可以表達成:
左右同時外積p2
矩陣形式可以表達成:
展開得到:
齊次方程式可以表達成:
矩陣形式:
可以表示為:
其中H矩陣有9個元素，8個自由度，因此需要8個點就能解出H矩陣
#### 函式詳解
1. 構造矩陣A
```
const int N = vP1.size();
cv::Mat A(2*N,9,CV_32F); // 2N*9
//遍歷所有特徵點
for(int i=0; i<N; i++)
{
//特徵點對的座標
const float u1 = vP1[i].x;
const float v1 = vP1[i].y;
const float u2 = vP2[i].x;
const float v2 = vP2[i].y;
//A矩陣賦值
A.at<float>(2*i,0) = 0.0;
A.at<float>(2*i,1) = 0.0;
A.at<float>(2*i,2) = 0.0;
A.at<float>(2*i,3) = -u1;
A.at<float>(2*i,4) = -v1;
A.at<float>(2*i,5) = -1;
A.at<float>(2*i,6) = v2*u1;
A.at<float>(2*i,7) = v2*v1;
A.at<float>(2*i,8) = v2;
A.at<float>(2*i+1,0) = u1;
A.at<float>(2*i+1,1) = v1;
A.at<float>(2*i+1,2) = 1;
A.at<float>(2*i+1,3) = 0.0;
A.at<float>(2*i+1,4) = 0.0;
A.at<float>(2*i+1,5) = 0.0;
A.at<float>(2*i+1,6) = -u2*u1;
A.at<float>(2*i+1,7) = -u2*v1;
A.at<float>(2*i+1,8) = -u2;
}
```
2. SVD分解，求出單應矩陣
```
cv::Mat u,w,vt;
cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
return vt.row(8).reshape(0, 3);
```
### Initializer::ComputeF21()
用8點法計算F矩陣
在[Initializer::FindFundamental()](#Initializer::FindFundamental())中被引用
#### 定義
```
cv::Mat Initializer::ComputeF21(const vector<cv::Point2f> &vP1, //參考幀歸一化後的特徵點
const vector<cv::Point2f> &vP2) //當前幀歸一化後的特徵點
```
#### 函式流程
1. 構造矩陣A
2. SVD分解，求出基礎矩陣
#### 知識點
1. 基礎矩陣
#### 函式詳解
1. 構造矩陣A
```
const int N = vP1.size();
cv::Mat A(2*N,9,CV_32F); // 2N*9
for(int i=0; i<N; i++)
{
const float u1 = vP1[i].x;
const float v1 = vP1[i].y;
const float u2 = vP2[i].x;
const float v2 = vP2[i].y;
A.at<float>(2*i,0) = 0.0;
A.at<float>(2*i,1) = 0.0;
A.at<float>(2*i,2) = 0.0;
A.at<float>(2*i,3) = -u1;
A.at<float>(2*i,4) = -v1;
A.at<float>(2*i,5) = -1;
A.at<float>(2*i,6) = v2*u1;
A.at<float>(2*i,7) = v2*v1;
A.at<float>(2*i,8) = v2;
A.at<float>(2*i+1,0) = u1;
A.at<float>(2*i+1,1) = v1;
A.at<float>(2*i+1,2) = 1;
A.at<float>(2*i+1,3) = 0.0;
A.at<float>(2*i+1,4) = 0.0;
A.at<float>(2*i+1,5) = 0.0;
A.at<float>(2*i+1,6) = -u2*u1;
A.at<float>(2*i+1,7) = -u2*v1;
A.at<float>(2*i+1,8) = -u2;
}
```
2. SVD分解，求出基礎矩陣
```
cv::Mat u,w,vt;
cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
return vt.row(8).reshape(0, 3); // v的最后一列
```
### Initializer::CheckHomography()
用重投影誤差計算單應矩陣的分數
在[Initializer::FindHomography()](#Initializer::FindHomography())中引用
#### 定義
```
float Initializer::CheckHomography(const cv::Mat &H21, //參考幀到當前幀的單應矩陣
const cv::Mat &H12, //當前幀到參考幀的單應矩陣
vector<bool> &vbMatchesInliers, //輸出特徵點對的內點標記
float sigma) //變異數，預設為1
```
#### 函式流程
1. 取得單應矩陣(H)的值
2. 取得參考幀與當前幀的匹配點對
3. 用單應矩陣記算重投影誤差
4. 用重投影誤差標記離群點，內點累積得分
5. 根據誤差將匹配點對分配為內點外點
#### 函式詳解
1. 取得單應矩陣(H)的值
```
const int N = mvMatches12.size();
// |h11 h12 h13|
// |h21 h22 h23|
// |h31 h32 h33|
//參考幀到當前幀
const float h11 = H21.at<float>(0,0);
const float h12 = H21.at<float>(0,1);
const float h13 = H21.at<float>(0,2);
const float h21 = H21.at<float>(1,0);
const float h22 = H21.at<float>(1,1);
const float h23 = H21.at<float>(1,2);
const float h31 = H21.at<float>(2,0);
const float h32 = H21.at<float>(2,1);
const float h33 = H21.at<float>(2,2);
// |h11inv h12inv h13inv|
// |h21inv h22inv h23inv|
// |h31inv h32inv h33inv|
//當前幀到參考幀
const float h11inv = H12.at<float>(0,0);
const float h12inv = H12.at<float>(0,1);
const float h13inv = H12.at<float>(0,2);
const float h21inv = H12.at<float>(1,0);
const float h22inv = H12.at<float>(1,1);
const float h23inv = H12.at<float>(1,2);
const float h31inv = H12.at<float>(2,0);
const float h32inv = H12.at<float>(2,1);
const float h33inv = H12.at<float>(2,2);
```
2. 取得參考幀與當前幀的匹配點對
```
vbMatchesInliers.resize(N);
float score = 0;
// 基於卡方檢驗計算出的閾值（假設測量有一個像素的偏差）
const float th = 5.991;
//信息矩陣，方差平方的倒數
const float invSigmaSquare = 1.0/(sigma*sigma);
// 遍歷N對特徵匹配點
for(int i=0; i<N; i++)
{
bool bIn = true;
//取得參考幀與當前幀的匹配點對
const cv::KeyPoint &kp1 = mvKeys1[mvMatches12[i].first];
const cv::KeyPoint &kp2 = mvKeys2[mvMatches12[i].second];
const float u1 = kp1.pt.x;
const float v1 = kp1.pt.y;
const float u2 = kp2.pt.x;
const float v2 = kp2.pt.y;
```
3. 用單應矩陣記算重投影誤差
```
// Reprojection error in first image
// x2in1 = H12*x2
// 將影像2中的特徵點單應到影像1中
// |u1| |h11inv h12inv h13inv||u2| |u2in1|
// |v1| = |h21inv h22inv h23inv||v2| = |v2in1|
// |1 | |h31inv h32inv h33inv||1 | | 1 |
//投影完後的座標
const float w2in1inv = 1.0/(h31inv*u2+h32inv*v2+h33inv);
const float u2in1 = (h11inv*u2+h12inv*v2+h13inv)*w2in1inv;
const float v2in1 = (h21inv*u2+h22inv*v2+h23inv)*w2in1inv;
// 計算重投影誤差
const float squareDist1 = (u1-u2in1)*(u1-u2in1)+(v1-v2in1)*(v1-v2in1);
// 根據方差歸一化誤差
const float chiSquare1 = squareDist1*invSigmaSquare;
```
4. 用重投影誤差標記離群點，內點累積得分
```
if(chiSquare1>th)
bIn = false;
else
score += th - chiSquare1; //誤差越大得分越低
// Reprojection error in second image
// x1in2 = H21*x1
// 將圖像1中的特徵點單應到圖像2中
const float w1in2inv = 1.0/(h31*u1+h32*v1+h33);
const float u1in2 = (h11*u1+h12*v1+h13)*w1in2inv;
const float v1in2 = (h21*u1+h22*v1+h23)*w1in2inv;
const float squareDist2 = (u2-u1in2)*(u2-u1in2)+(v2-v1in2)*(v2-v1in2);
const float chiSquare2 = squareDist2*invSigmaSquare;
if(chiSquare2>th)
bIn = false;
else
score += th - chiSquare2;
```
5. 根據誤差將匹配點對分配為內點外點
```
if(bIn)
vbMatchesInliers[i]=true;
else
vbMatchesInliers[i]=false;
}
return score;
```
### Initializer::CheckFundamental()
用重投影誤差計算基礎矩陣的分數
在[Initializer::FindFundamental()](#Initializer::FindFundamental())中引用
#### 定義
```
float Initializer::CheckFundamental(const cv::Mat &F21, //當前幀與參考幀之間的基礎矩陣
vector<bool> &vbMatchesInliers, //輸出特徵點對的內點標記
float sigma) //方差
```
#### 函式流程
1. 取得基礎矩陣(F)的值
2. 取得參考幀與當前幀的匹配點對
3. 用基礎矩陣計算參考幀投影在當前幀得到的極線
4. 計算誤差
5. 標記內點外點
6. 用基礎矩陣計算當前幀投影在參考幀得到的極線
7. 計算誤差
8. 根據誤差將匹配點對分配為內點外點
#### 函式詳解
1. 取得基礎矩陣(F)的值
```
const int N = mvMatches12.size();
const float f11 = F21.at<float>(0,0);
const float f12 = F21.at<float>(0,1);
const float f13 = F21.at<float>(0,2);
const float f21 = F21.at<float>(1,0);
const float f22 = F21.at<float>(1,1);
const float f23 = F21.at<float>(1,2);
const float f31 = F21.at<float>(2,0);
const float f32 = F21.at<float>(2,1);
const float f33 = F21.at<float>(2,2);
vbMatchesInliers.resize(N);
float score = 0;
// 基於卡方檢驗計算出的閾值（假設測量有一個像素的偏差）
const float th = 3.841;
const float thScore = 5.991;
const float invSigmaSquare = 1.0/(sigma*sigma);
```
2. 取得參考幀與當前幀的匹配點對
```
for(int i=0; i<N; i++)
{
bool bIn = true;
const cv::KeyPoint &kp1 = mvKeys1[mvMatches12[i].first];
const cv::KeyPoint &kp2 = mvKeys2[mvMatches12[i].second];
const float u1 = kp1.pt.x;
const float v1 = kp1.pt.y;
const float u2 = kp2.pt.x;
const float v2 = kp2.pt.y;
```
3. 用基礎矩陣計算參考幀投影在當前幀得到的極線
```
// Reprojection error in second image
// l2=F21x1=(a2,b2,c2)
// F21x1可以算出x1在圖像中x2對應的線l
const float a2 = f11*u1+f12*v1+f13;
const float b2 = f21*u1+f22*v1+f23;
const float c2 = f31*u1+f32*v1+f33;
```
4. 計算誤差
```
// x2應該在l這條線上:x2點乘l = 0
const float num2 = a2*u2+b2*v2+c2;
const float squareDist1 = num2*num2/(a2*a2+b2*b2); // 點到線的幾何距離 的平方
const float chiSquare1 = squareDist1*invSigmaSquare;
```
5. 標記內點外點
```
if(chiSquare1>th)
bIn = false;
else
score += thScore - chiSquare1;
// Reprojection error in second image
// l1 =x2tF21=(a1,b1,c1)
```
6. 用基礎矩陣計算當前幀投影在參考幀得到的極線
```
const float a1 = f11*u2+f21*v2+f31;
const float b1 = f12*u2+f22*v2+f32;
const float c1 = f13*u2+f23*v2+f33;
```
7. 計算誤差
```
const float num1 = a1*u1+b1*v1+c1;
const float squareDist2 = num1*num1/(a1*a1+b1*b1);
const float chiSquare2 = squareDist2*invSigmaSquare;
```
8. 根據誤差將匹配點對分配為內點外點
```
if(chiSquare2>th)
bIn = false;
else
score += thScore - chiSquare2;
if(bIn)
vbMatchesInliers[i]=true;
else
vbMatchesInliers[i]=false;
}
return score;
```
### Initializer::ReconstructH()
用單應矩陣得到旋轉矩陣及平移矩陣
在[Initializer::Initialize()](#Initializer::Initialize())中引用
其中算法根據論文 [motion and structure from motion in a piecewise planar environment](https://www.researchgate.net/publication/243764888_Motion_and_Structure_from_Motion_in_a_Piecewise_Planar_Environment)
#### 定義
```
bool Initializer::ReconstructH(vector<bool> &vbMatchesInliers, //輸入成功匹配特徵點對的內點標記
cv::Mat &H21, //輸入RANSAC計算後的單應矩陣
cv::Mat &K, //輸入相機內參
cv::Mat &R21, //輸出參考幀1到當前幀2的旋轉矩陣
cv::Mat &t21, //輸出參考幀1到當前幀2的平移矩陣
vector<cv::Point3f> &vP3D, //三角測量後的空間座標
vector<bool> &vbTriangulated, //是否成功三角化
float minParallax, //測量有效的最好視差角
int minTriangulated) //最少需要的三角化點數量
```
#### 函式流程
1. 統計匹配的特徵點對中，屬於內點的數量
2. SVD分解單應矩陣
3. 得到矩陣的奇異值
4. 定義旋轉，平移，空間向量
5. 計算法向量
6. 計算解>0的四種旋轉矩陣
7. 計算解<0的四種旋轉矩陣
8. 對八組R,T進行驗證
9. 通過判斷最優是否明顯好於次優，從而判斷該次矩陣H分解是否成功
#### 函式詳解
1. 統計匹配的特徵點對中，屬於內點的數量
```
int N=0;
for(size_t i=0, iend = vbMatchesInliers.size() ; i<iend; i++)
if(vbMatchesInliers[i])
N++;
```
2. SVD分解單應矩陣
```
// We recover 8 motion hypotheses using the method of Faugeras et al.
// Motion and structure from motion in a piecewise planar environment.
// International Journal of Pattern Recognition and Artificial Intelligence, 1988
// step1：
// 因為特徵點是圖像坐標系，所以講H矩陣由相機坐標系換算到圖像坐標系
cv::Mat invK = K.inv();
cv::Mat A = invK*H21*K;
cv::Mat U,w,Vt,V;
cv::SVD::compute(A,w,U,Vt,cv::SVD::FULL_UV);
V=Vt.t();
```
3. 得到矩陣的奇異值
```
float s = cv::determinant(U)*cv::determinant(Vt);
float d1 = w.at<float>(0);
float d2 = w.at<float>(1);
float d3 = w.at<float>(2);
// SVD分解的正常情況是特徵值降序排列
if(d1/d2<1.00001 || d2/d3<1.00001)
{
return false;
}
```
4. 定義旋轉，平移，空間向量
```
vector<cv::Mat> vR, vt, vn;
vR.reserve(8);
vt.reserve(8);
vn.reserve(8);
```
5. 計算法向量
```
// n'=[x1 0 x3] 4 posibilities e1=e3=1, e1=1 e3=-1, e1=-1 e3=1, e1=e3=-1
// 法向量n'= [x1 0 x3]
float aux1 = sqrt((d1*d1-d2*d2)/(d1*d1-d3*d3));
float aux3 = sqrt((d2*d2-d3*d3)/(d1*d1-d3*d3));
float x1[] = {aux1,aux1,-aux1,-aux1};
float x3[] = {aux3,-aux3,aux3,-aux3};
```
6. 計算解>0的四種旋轉矩陣
```
// 計算 sin(theta)和cos(theta)，case d'=d2
float aux_stheta = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1+d3)*d2);
float ctheta = (d2*d2+d1*d3)/((d1+d3)*d2);
float stheta[] = {aux_stheta, -aux_stheta, -aux_stheta, aux_stheta};
// 計算四種旋轉矩陣R，t
// 計算旋轉矩陣 R
// | ctheta 0 -aux_stheta| | aux1|
// Rp = | 0 1 0 | tp = | 0 |
// | aux_stheta 0 ctheta | |-aux3|
// | ctheta 0 aux_stheta| | aux1|
// Rp = | 0 1 0 | tp = | 0 |
// |-aux_stheta 0 ctheta | | aux3|
// | ctheta 0 aux_stheta| |-aux1|
// Rp = | 0 1 0 | tp = | 0 |
// |-aux_stheta 0 ctheta | |-aux3|
// | ctheta 0 -aux_stheta| |-aux1|
// Rp = | 0 1 0 | tp = | 0 |
// | aux_stheta 0 ctheta | | aux3|
//遍歷四種解
for(int i=0; i<4; i++)
{
cv::Mat Rp=cv::Mat::eye(3,3,CV_32F);
Rp.at<float>(0,0)=ctheta;
Rp.at<float>(0,2)=-stheta[i];
Rp.at<float>(2,0)=stheta[i];
Rp.at<float>(2,2)=ctheta;
cv::Mat R = s*U*Rp*Vt;
vR.push_back(R);
cv::Mat tp(3,1,CV_32F);
tp.at<float>(0)=x1[i];
tp.at<float>(1)=0;
tp.at<float>(2)=-x3[i];
tp*=d1-d3;
// 這裡雖然對t有歸一化，並沒有決定單目整個SLAM過程的尺度
// 因為CreateInitialMapMonocular函數對3D點深度會縮放，然後反過來對 t 有改變
cv::Mat t = U*tp;
vt.push_back(t/cv::norm(t));
cv::Mat np(3,1,CV_32F);
np.at<float>(0)=x1[i];
np.at<float>(1)=0;
np.at<float>(2)=x3[i];
cv::Mat n = V*np;
if(n.at<float>(2)<0)
n=-n;
vn.push_back(n);
}
```
7. 計算解<0的四種旋轉矩陣
```
float aux_sphi = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1-d3)*d2);
float cphi = (d1*d3-d2*d2)/((d1-d3)*d2);
float sphi[] = {aux_sphi, -aux_sphi, -aux_sphi, aux_sphi};
// step3.4：計算四種旋轉矩陣R，t
// 計算旋轉矩陣 R‘，計算ppt中公式21
for(int i=0; i<4; i++)
{
cv::Mat Rp=cv::Mat::eye(3,3,CV_32F);
Rp.at<float>(0,0)=cphi;
Rp.at<float>(0,2)=sphi[i];
Rp.at<float>(1,1)=-1;
Rp.at<float>(2,0)=sphi[i];
Rp.at<float>(2,2)=-cphi;
cv::Mat R = s*U*Rp*Vt;
vR.push_back(R);
cv::Mat tp(3,1,CV_32F);
tp.at<float>(0)=x1[i];
tp.at<float>(1)=0;
tp.at<float>(2)=x3[i];
tp*=d1+d3;
cv::Mat t = U*tp;
vt.push_back(t/cv::norm(t));
cv::Mat np(3,1,CV_32F);
np.at<float>(0)=x1[i];
np.at<float>(1)=0;
np.at<float>(2)=x3[i];
cv::Mat n = V*np;
if(n.at<float>(2)<0)
n=-n;
vn.push_back(n);
}
```
8. 對八組R,T進行驗證
```
//最好的點
int bestGood = 0;
//第二好的點
int secondBestGood = 0;
//最優解的索引
int bestSolutionIdx = -1;
//最大視差角
float bestParallax = -1;
//最好的解對應的特徵點對三角化的結果
vector<cv::Point3f> bestP3D;
//最好的解對應的可以被三角化的特徵點的標記
vector<bool> bestTriangulated;
for(size_t i=0; i<8; i++)
{
//這組解的較大視差角
float parallaxi;
//三角化後得空間座標
vector<cv::Point3f> vP3Di;
//特徵點是否被三角化的標記
vector<bool> vbTriangulatedi;
int nGood = CheckRT(vR[i],vt[i],mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K,vP3Di, 4.0*mSigma2, vbTriangulatedi, parallaxi);
// 保留最優的和次優的
if(nGood>bestGood)
{
secondBestGood = bestGood;
bestGood = nGood;
bestSolutionIdx = i;
bestParallax = parallaxi;
bestP3D = vP3Di;
bestTriangulated = vbTriangulatedi;
}
else if(nGood>secondBestGood)
{
secondBestGood = nGood;
}
}
```
跳到[Initializer::CheckRT()](#Initializer::CheckRT)
9. 通過判斷最優是否明顯好於次優，從而判斷該次矩陣H分解是否成功
```
if(secondBestGood<0.75*bestGood && bestParallax>=minParallax && bestGood>minTriangulated && bestGood>0.9*N)
{
//從最佳解提取R,T
vR[bestSolutionIdx].copyTo(R21);
vt[bestSolutionIdx].copyTo(t21);
//獲得最佳解時，成功三角化的三維點，之後會作為初始地圖點使用
vP3D = bestP3D;
//成功三角化的特徵點標記
vbTriangulated = bestTriangulated;
return true;
}
return false;
```
### Initializer::ReconstructF()
用基礎矩陣得到旋轉矩陣及平移矩陣
在[Initializer::Initialize()](#Initializer::Initialize())中引用
#### 定義
```
bool Initializer::ReconstructF(vector<bool> &vbMatchesInliers, //輸入匹配好的特徵點對內點標記
cv::Mat &F21, //輸入參考幀到當前幀的基礎矩陣
cv::Mat &K, //內參矩陣
cv::Mat &R21, //旋轉矩陣
cv::Mat &t21, //平移矩陣
vector<cv::Point3f> &vP3D, //輸出三角化後特徵點的空間座標
vector<bool> &vbTriangulated, //三角化成功的標記
float minParallax, //認為三角化有效的最小視差角
int minTriangulated) //最小三角化點的數量
```
#### 函式流程
1. 統計內點數量
2. 根據基礎矩陣及內參得到本質矩陣
3. 從本質矩陣解兩個R與兩個T，共四組解
4. 用同個匹配點檢查四組解
5. 檢查解的合法性
6. 尋找最優解
#### 函式詳解
1. 統計內點數量
```
int N=0;
for(size_t i=0, iend = vbMatchesInliers.size() ; i<iend; i++)
if(vbMatchesInliers[i])
N++;
```
2. 根據基礎矩陣及內參得到本質矩陣
```
// Compute Essential Matrix from Fundamental Matrix
cv::Mat E21 = K.t()*F21*K;
```
3. 從本質矩陣解兩個R與兩個T，共四組解
```
cv::Mat R1, R2, t;
// Recover the 4 motion hypotheses
// 雖然這個函數對t有歸一化，但並沒有決定單目整個SLAM過程的尺度
// 因為CreateInitialMapMonocular 函數對3D點深度會縮放，然後反過來對 t 有改變
DecomposeE(E21,R1,R2,t);
```
4. 用同個匹配點檢查四組解
```
cv::Mat t1=t;
cv::Mat t2=-t;
// Reconstruct with the 4 hyphoteses and check
//對同一點進行三角化之後的空間座標
vector<cv::Point3f> vP3D1, vP3D2, vP3D3, vP3D4;
//四組解三角化之後的空間座標的有效標記
vector<bool> vbTriangulated1,vbTriangulated2,vbTriangulated3, vbTriangulated4;
//四種解的較大視差角
float parallax1,parallax2, parallax3, parallax4;
int nGood1 = CheckRT(R1,t1,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D1, 4.0*mSigma2, vbTriangulated1, parallax1);
int nGood2 = CheckRT(R2,t1,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D2, 4.0*mSigma2, vbTriangulated2, parallax2);
int nGood3 = CheckRT(R1,t2,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D3, 4.0*mSigma2, vbTriangulated3, parallax3);
int nGood4 = CheckRT(R2,t2,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D4, 4.0*mSigma2, vbTriangulated4, parallax4);
```
5. 檢查解的合法性
```
//找最大可三角化測量的點的數目
int maxGood = max(nGood1,max(nGood2,max(nGood3,nGood4)));
//輸出值
R21 = cv::Mat();
t21 = cv::Mat();
// 最小可以三角化恢復三維點的個數
int nMinGood = max(static_cast<int>(0.9*N),minTriangulated);
//如果多個解同時滿足就有問題
int nsimilar = 0;
if(nGood1>0.7*maxGood)
nsimilar++;
if(nGood2>0.7*maxGood)
nsimilar++;
if(nGood3>0.7*maxGood)
nsimilar++;
if(nGood4>0.7*maxGood)
nsimilar++;
// If there is not a clear winner or not enough triangulated points reject initialization
// 四個結果中如果沒有明顯的最優結果，則返回失敗
if(maxGood<nMinGood || nsimilar>1)
{
return false;
}
```
6. 尋找最優解
```
// If best reconstruction has enough parallax initialize
// 找最好的解在哪裡發生的
if(maxGood==nGood1)
{
//是否大於最小視差
if(parallax1>minParallax)
{
vP3D = vP3D1;
vbTriangulated = vbTriangulated1;
R1.copyTo(R21);
t1.copyTo(t21);
return true;
}
}else if(maxGood==nGood2)
{
if(parallax2>minParallax)
{
vP3D = vP3D2;
vbTriangulated = vbTriangulated2;
R2.copyTo(R21);
t1.copyTo(t21);
return true;
}
}else if(maxGood==nGood3)
{
if(parallax3>minParallax)
{
vP3D = vP3D3;
vbTriangulated = vbTriangulated3;
R1.copyTo(R21);
t2.copyTo(t21);
return true;
}
}else if(maxGood==nGood4)
{
if(parallax4>minParallax)
{
vP3D = vP3D4;
vbTriangulated = vbTriangulated4;
R2.copyTo(R21);
t2.copyTo(t21);
return true;
}
}
return false;
```
CheckRT: 跳到[Initializer::CheckRT()](#Initializer::CheckRT())
DecomposeE: 跳到[Initializer::DecomposeE()](Initializer::DecomposeE()
### Initializer::CheckRT()
用R,T來對特徵匹配點做三角化，並根據三角化結果判斷R,T的合法性
在[Initializer::ReconstructH()](#Initializer::ReconstructH())及[Initializer::ReconstructF()](#Initializer::ReconstructF)中引用
#### 定義
```
int Initializer::CheckRT(const cv::Mat &R, //旋轉矩陣
const cv::Mat &t, //平移矩陣
const vector<cv::KeyPoint> &vKeys1, //參考幀特徵點
const vector<cv::KeyPoint> &vKeys2, //當前幀特徵點
const vector<Match> &vMatches12, //特徵匹配關係
vector<bool> &vbMatchesInliers, //內點標記
const cv::Mat &K, //內參矩陣
vector<cv::Point3f> &vP3D, //三角化後得空間座標
float th2, //三角化中允許的最大重投影誤差
vector<bool> &vbGood, //特徵點是否被成功進行三角化的標記
float ¶llax) //這組解的較大視差角
```
#### 函式流程
1. 提取參數
2. 計算相機的投影矩陣[知識點1]
3. 特徵點三角化
4. 檢查三角化後的三維點是否合法
5. 檢查三角化後的深度及視差是否合法
6. 計算第一個攝影機的重投影誤差
7. 計算第二個攝影機的重投影誤差
8. 統計經過檢驗的3D點個數，記錄3D點視差角
9. 得到3D點中較大的視差角
#### 知識點
1. 相機投影
投影點是p1,p2
#### 函式詳解
1. 提取參數
```
// Calibration parameters
const float fx = K.at<float>(0,0);
const float fy = K.at<float>(1,1);
const float cx = K.at<float>(0,2);
const float cy = K.at<float>(1,2);
//儲存能成功三角化的特徵點
vbGood = vector<bool>(vKeys1.size(),false);
//儲存空間座標點
vP3D.resize(vKeys1.size());
//儲存美對特徵點的視差
vector<float> vCosParallax;
vCosParallax.reserve(vKeys1.size());
```
2. 計算相機的投影矩陣
```
// Camera 1 Projection Matrix K[I|0]
// 以第一個相機的光心作為世界坐標系
//投影矩陣
cv::Mat P1(3,4,CV_32F,cv::Scalar(0));
K.copyTo(P1.rowRange(0,3).colRange(0,3));
// 第一個相機的光心在世界坐標系下的坐標，設為原點
cv::Mat O1 = cv::Mat::zeros(3,1,CV_32F);
// Camera 2 Projection Matrix K[R|t]
// 得到第二個相機的投影矩陣
cv::Mat P2(3,4,CV_32F);
R.copyTo(P2.rowRange(0,3).colRange(0,3));
t.copyTo(P2.rowRange(0,3).col(3));
P2 = K*P2;
// 第二個相機的光心在世界坐標系下的坐標
cv::Mat O2 = -R.t()*t;
int nGood=0;
```
3. 特徵點三角化
```
//遍歷所有特徵點對
for(size_t i=0, iend=vMatches12.size();i<iend;i++)
{
//跳過外點
if(!vbMatchesInliers[i])
continue;
// kp1和kp2是匹配特徵點
const cv::KeyPoint &kp1 = vKeys1[vMatches12[i].first];
const cv::KeyPoint &kp2 = vKeys2[vMatches12[i].second];
//儲存三圍點的座標
cv::Mat p3dC1;
// 利用三角法恢復三維點p3dC1
Triangulate(kp1,kp2, //特徵點
P1,P2, //投影矩陣
p3dC1); //輸出三角測量後特徵點的座標
```
跳到[Initializer::Triangulate()](#Initializer::Triangulate())
4. 檢查三角化後的三維點是否合法
```
//有值是無窮大就代表失敗
if(!isfinite(p3dC1.at<float>(0)) || !isfinite(p3dC1.at<float>(1)) || !isfinite(p3dC1.at<float>(2)))
{
vbGood[vMatches12[i].first]=false;
continue;
}
```
5. 檢查三角化後的深度及視差是否合法
```
// Check parallax
// 計算視差角餘弦值
cv::Mat normal1 = p3dC1 - O1;
//深度
float dist1 = cv::norm(normal1);
cv::Mat normal2 = p3dC1 - O2;
float dist2 = cv::norm(normal2);
//夾角
float cosParallax = normal1.dot(normal2)/(dist1*dist2);
// 3D點深度為負，在第一個攝影機後方，不合法
if(p3dC1.at<float>(2)<=0 && cosParallax<0.99998)
continue;
// Check depth in front of second camera (only if enough parallax, as "infinite" points can easily go to negative depth)
//3D點深度為負，在第二個攝影機後方，不合法
cv::Mat p3dC2 = R*p3dC1+t;
if(p3dC2.at<float>(2)<=0 && cosParallax<0.99998)
continue;
```
6. 計算第一個攝影機的重投影誤差
```
// Check reprojection error in first image
// 計算3D點在第一個圖像上的投影誤差
float im1x, im1y;
float invZ1 = 1.0/p3dC1.at<float>(2);
im1x = fx*p3dC1.at<float>(0)*invZ1+cx;
im1y = fy*p3dC1.at<float>(1)*invZ1+cy;
float squareError1 = (im1x-kp1.pt.x)*(im1x-kp1.pt.x)+(im1y-kp1.pt.y)*(im1y-kp1.pt.y);
// 一般視差角比較小時重投影誤差比較大
if(squareError1>th2)
continue;
```
7. 計算第二個攝影機的重投影誤差
```
// Check reprojection error in second image
// 計算3D點在第二個圖像上的投影誤差
float im2x, im2y;
float invZ2 = 1.0/p3dC2.at<float>(2);
im2x = fx*p3dC2.at<float>(0)*invZ2+cx;
im2y = fy*p3dC2.at<float>(1)*invZ2+cy;
float squareError2 = (im2x-kp2.pt.x)*(im2x-kp2.pt.x)+(im2y-kp2.pt.y)*(im2y-kp2.pt.y);
// 一般視差角比較小時重投影誤差比較大
if(squareError2>th2)
continue;
```
8. 統計經過檢驗的3D點個數，記錄3D點視差角
```
vCosParallax.push_back(cosParallax);
vP3D[vMatches12[i].first] = cv::Point3f(p3dC1.at<float>(0),p3dC1.at<float>(1),p3dC1.at<float>(2));
nGood++;
//判斷視差 >0.36度
if(cosParallax<0.99998)
vbGood[vMatches12[i].first]=true;
}
```
9. 得到3D點中較大的視差角
```
if(nGood>0)
{
// 從小到大排序
sort(vCosParallax.begin(),vCosParallax.end());
//如果有效點小於50個，就取一個最小的視差角，cos值最大，如果有效點大於50個，就取第50個
size_t idx = min(50,int(vCosParallax.size()-1));
//轉弧度
parallax = acos(vCosParallax[idx])*180/CV_PI;
}
else
parallax=0;
return nGood;
```
### Initializer::Triangulate()
用三角測量計算三維點座標
在[Initializer::CheckRT()](#Initializer::CheckRT())中引用
#### 定義
```
void Initializer::Triangulate(const cv::KeyPoint &kp1, //特徵點
const cv::KeyPoint &kp2, //特徵點
const cv::Mat &P1, //投影矩陣p1
const cv::Mat &P2, //投影矩陣p2
cv::Mat &x3D) //三維點
```
#### 函式流程
1. 計算要來分解的矩陣
2. svd分解
#### 函式詳解
1. 計算要來分解的矩陣
```
cv::Mat A(4,4,CV_32F);
A.row(0) = kp1.pt.x*P1.row(2)-P1.row(0);
A.row(1) = kp1.pt.y*P1.row(2)-P1.row(1);
A.row(2) = kp2.pt.x*P2.row(2)-P2.row(0);
A.row(3) = kp2.pt.y*P2.row(2)-P2.row(1);
```
2. svd分解
```
cv::Mat u,w,vt;
cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
x3D = vt.row(3).t();
x3D = x3D.rowRange(0,3)/x3D.at<float>(3);
```
### Initializer::DecomposeE()
分解本質矩陣得到這4組解分別為[R1,t],[R1,-t],[R2,t],[R2,-t]
在[Initializer::ReconstructF()](#Initializer::ReconstructF())中引用
#### 定義
```
void Initializer::DecomposeE(const cv::Mat &E, //本質矩陣
cv::Mat &R1, //旋轉矩陣1
cv::Mat &R2, //旋轉矩陣2
cv::Mat &t) //平移矩陣
```
#### 函式流程
1. 對本質矩陣進行奇異值分解
2. 建構旋轉矩陣
#### 函式詳解
1. 對本質矩陣進行奇異值分解
```
cv::Mat u,w,vt;
cv::SVD::compute(E,w,u,vt);
// 對 t 歸一化，但是這個地方並沒有決定單目整個SLAM過程的尺度
// 因為CreateInitialMapMonocular函數對3D點深度會縮放，然後反過來對 t 有改變
u.col(2).copyTo(t);
t=t/cv::norm(t);
```
2. 建構旋轉矩陣
```
cv::Mat W(3,3,CV_32F,cv::Scalar(0));
W.at<float>(0,1)=-1;
W.at<float>(1,0)=1;
W.at<float>(2,2)=1;
R1 = u*W*vt;
if(cv::determinant(R1)<0) // 旋轉矩陣有行列式為1的約束
R1=-R1;
R2 = u*W.t()*vt;
if(cv::determinant(R2)<0)
R2=-R2;
```
Sign in with Wallet
Connect another wallet