基本流程
单目初始化程序存储在Initializer.cc中
需要注意，对于双目/RGB-D相机，初始化时，由于可以直接获得相机的深度信息，因此无需求H/F，直接作为关键帧插入就行。
使用RANSAC+DLT求解H，RANSAC+八点法求解F。选择评分高的，然后进行分解获得R，t。分解时会有多组解，选择最多深度为正的路标点的解。
构建初始化器
nitializer::Initializer(const Frame &ReferenceFrame, float sigma, int iterations)
特征点坐标归一化处理
前辈发现计算单应矩阵时变换特征点的坐标会得到更好的效果，包括坐标的平移和尺度缩放，并且这一步骤必须放在DLT之前。DLT之后再还原到原坐标系。
（1）将点进行平移使其形心（x,y的均值）位于原点。
（2）对点进行缩放使特征点到原点的距离为根号2，即所有点“平均”位于（1，1，1）
（3）对两幅图进行独立的上述变换
计算单应矩阵
cv::Mat Initializer::ComputeH21(
const vector<cv::Point2f> &vP1, //归一化后的点, in reference frame
const vector<cv::Point2f> &vP2) //归一化后的点, in current frame
有关SVD分解过程可以参考
https://blog.csdn.net/sinat_28309919/article/details/80134985
奇异值分解后，V的最后一列向量即为解。
// 定义输出变量，u是左边的正交矩阵U， w为奇异矩阵，vt中的t表示是右正交矩阵V的转置
cv::Mat u,w,vt;
//使用opencv提供的进行奇异值分解的函数
cv::SVDecomp(A, //输入，待进行奇异值分解的矩阵
w, //输出，奇异值矩阵
u, //输出，矩阵U
vt, //输出，矩阵V^T
cv::SVD::MODIFY_A | //输入，MODIFY_A是指允许计算函数可以修改待分解的矩阵，官方文档上说这样可以加快计算速度、节省内存
cv::SVD::FULL_UV); //FULL_UV=把U和VT补充成单位正交方阵
// 返回最小奇异值所对应的右奇异向量
// 注意前面说的是右奇异值矩阵的最后一列，但是在这里因为是vt，转置后了，所以是行；由于A有9列数据，故最后一列的下标为8
return vt.row(8).reshape(0, //转换后的通道数，这里设置为0表示是与前面相同
3); //转换后的行数,对应V的最后一列
计算H的评分
使用对称转移误差为评判标准。据此来判断特征点是内点还是外点。
currentScore = CheckHomography(H21i, H12i, //输入，单应矩阵的计算结果
vbCurrentInliers, //输出，特征点对的Inliers标记
mSigma);
通过H矩阵，进行参考帧和当前帧之间的双向投影，并计算起加权最小二乘投影误差
（1）计算重投影误差：
const float squareDist1 = (u1 - u2in1) * (u1 - u2in1) + (v1 - v2in1) * (v1 - v2in1);
const float chiSquare1 = squareDist1 * invSigmaSquare;
（2）判断是内点还是外点
误差小于阈值的话，为内点。内点的评分与误差的大小有关，误差越大，评分越小。
// Step 2.3 用阈值标记离群点，内点的话累加得分
if(chiSquare1>th)
bIn = false;
else
// th为设定的阈值，误差越大，得分越低
score += th - chiSquare1;
设定内点：
// Step 2.4 如果从img2 到 img1 和 从img1 到img2的重投影误差均满足要求，则说明是Inlier point
if(bIn)
vbMatchesInliers[i]=true;
else
vbMatchesInliers[i]=false;
寻找单应矩阵
void Initializer::FindHomography(vector<bool> &vbMatchesInliers, float &score, cv::Mat &H21)
对特征点坐标进行归一化处理
将当前帧和参考帧中的特征点坐标进行归一化。
//归一化后的参考帧1和当前帧2中的特征点坐标
vector<cv::Point2f> vPn1, vPn2;
// 记录各自的归一化矩阵
cv::Mat T1, T2;
Normalize(mvKeys1,vPn1, T1);
Normalize(mvKeys2,vPn2, T2);
取出RANSAC获得的随机特征点对的索引
for(size_t j=0; j<8; j++)
{
//从mvSets中获取当前次迭代的某个特征点对的索引信息
int idx = mvSets[it][j];
// vPn1i和vPn2i为匹配的特征点对的归一化后的坐标
// 首先根据这个特征点对的索引信息分别找到两个特征点在各自图像特征点向量中的索引，然后读取其归一化之后的特征点坐标
vPn1i[j] = vPn1[mvMatches12[idx].first]; //first存储在参考帧1中的特征点索引
vPn2i[j] = vPn2[mvMatches12[idx].second]; //second存储在参考帧1中的特征点索引
}//读取8对特征点的归一化之后的坐标
计算单应矩阵
&emps;需要注意，这里计算的单应矩阵使用的是归一化坐标。然后要恢复到原始坐标下。
cv::Mat Hn = ComputeH21(vPn1i,vPn2i);
// 单应矩阵原理：X2=H21*X1，其中X1,X2 为归一化后的特征点
// 特征点归一化：vPn1 = T1 * mvKeys1, vPn2 = T2 * mvKeys2 得到:T2 * mvKeys2 = Hn * T1 * mvKeys1
// 进一步得到:mvKeys2 = （T2.inv * Hn * T1） * mvKeys1
H21i = T2inv*Hn*T1;
//然后计算逆
H12i = H21i.inv();
计算该次迭代获得的H的评分
// Step 4 利用重投影误差为当次RANSAC的结果评分
currentScore = CheckHomography(H21i, H12i, //输入，单应矩阵的计算结果
vbCurrentInliers, //输出，特征点对的Inliers标记
mSigma);
保存最优的H
// Step 5 更新具有最优评分的单应矩阵计算结果,并且保存所对应的特征点对的内点标记
if(currentScore>score)
{
//如果当前的结果得分更高，那么就更新最优计算结果
H21 = H21i.clone();
//保存匹配好的特征点对的Inliers标记
vbMatchesInliers = vbCurrentInliers;
//更新历史最优评分
score = currentScore;
}
计算基础矩阵
cv::Mat Initializer::ComputeF21(
const vector<cv::Point2f> &vP1, //归一化后的点, in reference frame
const vector<cv::Point2f> &vP2) //归一化后的点, in current frame
SVD分解，D的最后一列即为F的值
//存储奇异值分解结果的变量
cv::Mat u,w,vt;
// 定义输出变量，u是左边的正交矩阵U， w为奇异矩阵，vt中的t表示是右正交矩阵V的转置
cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
// 转换成基础矩阵的形式
cv::Mat Fpre = vt.row(8).reshape(0, 3); // v的最后一列
//基础矩阵的秩为2,而我们不敢保证计算得到的这个结果的秩为2,所以需要通过第二次奇异值分解,来强制使其秩为2
// 对初步得来的基础矩阵进行第2次奇异值分解
cv::SVDecomp(Fpre,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
// 秩2约束，强制将第3个奇异值设置为0
w.at<float>(2)=0;
// 重新组合好满足秩约束的基础矩阵，作为最终计算结果返回
return u*cv::Mat::diag(w)*vt;
计算F的评分
误差为像素点到极线的距离。
通过点到极线的距离计算误差
误差小于阈值的话，设为内点。误差越小，贡献越大。
// Step 2.2 计算 img1 上的点在 img2 上投影得到的极线 l2 = F21 * p1 = (a2,b2,c2)
const float a2 = f11*u1+f12*v1+f13;
const float b2 = f21*u1+f22*v1+f23;
const float c2 = f31*u1+f32*v1+f33;
// Step 2.3 计算误差 e = (a * p2.x + b * p2.y + c) / sqrt(a * a + b * b)
const float num2 = a2*u2+b2*v2+c2;
const float squareDist1 = num2*num2/(a2*a2+b2*b2);
// 带权重误差
const float chiSquare1 = squareDist1*invSigmaSquare;
// Step 2.4 误差大于阈值就说明这个点是Outlier
// ? 为什么判断阈值用的 th（1自由度），计算得分用的thScore（2自由度）
// ? 可能是为了和CheckHomography 得分统一？
if(chiSquare1>th)
bIn = false;
else
// 误差越大，得分越低
score += thScore - chiSquare1;
寻找基础矩阵
void Initializer::FindFundamental(vector<bool> &vbMatchesInliers, float &score, cv::Mat &F21)
取出RANSAC获得的特征点索引
for(int j=0; j<8; j++)
{
int idx = mvSets[it][j];
// vPn1i和vPn2i为匹配的特征点对的归一化后的坐标
// 首先根据这个特征点对的索引信息分别找到两个特征点在各自图像特征点向量中的索引，然后读取其归一化之后的特征点坐标
vPn1i[j] = vPn1[mvMatches12[idx].first]; //first存储在参考帧1中的特征点索引
vPn2i[j] = vPn2[mvMatches12[idx].second]; //second存储在参考帧1中的特征点索引
}
使用八点法计算基础矩阵F
// Step 3 八点法计算基础矩阵
cv::Mat Fn = ComputeF21(vPn1i,vPn2i);
// 基础矩阵约束：p2^t*F21*p1 = 0，其中p1,p2 为齐次化特征点坐标
// 特征点归一化：vPn1 = T1 * mvKeys1, vPn2 = T2 * mvKeys2
// 根据基础矩阵约束得到:(T2 * mvKeys2)^t* Hn * T1 * mvKeys1 = 0
// 进一步得到:mvKeys2^t * T2^t * Hn * T1 * mvKeys1 = 0
F21i = T2t*Fn*T1;
获得该基础矩阵的评分
currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma);
更新最优的基础矩阵
if(currentScore>score)
{
//如果当前的结果得分更高，那么就更新最优计算结果
F21 = F21i.clone();
vbMatchesInliers = vbCurrentInliers;
score = currentScore;
}
初始化主函数
Tracking.cc的947行
（1）匹配特征点
（2）获得基础矩阵与单应矩阵
（3）选择最佳的来恢复两帧间的位姿变换。
（4）三角化获得路标点
/**
* @param[in] CurrentFrame 当前帧，也就是SLAM意义上的第二帧
* @param[in] vMatches12 当前帧（2）和参考帧（1）图像中特征点的匹配关系
* vMatches12[i]解释：i表示帧1中关键点的索引值，vMatches12[i]的值为帧2的关键点索引值
* 没有匹配关系的话，vMatches12[i]值为 -1
* @param[in & out] R21 相机从参考帧到当前帧的旋转
* @param[in & out] t21 相机从参考帧到当前帧的平移
* @param[in & out] vP3D 三角化测量之后的三维地图点
* @param[in & out] vbTriangulated 标记三角化点是否有效，有效为true
* @return true 该帧可以成功初始化，返回true
* @return false 该帧不满足初始化条件，返回false
*/
bool Initializer::Initialize(const Frame &CurrentFrame, const vector<int> &vMatches12, cv::Mat &R21, cv::Mat &t21,
vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated)
获得两帧间的特征点匹配关系
for(size_t i=0, iend=vMatches12.size();i<iend; i++)
{
//vMatches12[i]解释：i表示帧1中关键点的索引值，vMatches12[i]的值为帧2的关键点索引值
//没有匹配关系的话，vMatches12[i]值为 -1
if(vMatches12[i]>=0)
{
//mvMatches12 中只记录有匹配关系的特征点对的索引值
//i表示帧1中关键点的索引值，vMatches12[i]的值为帧2的关键点索引值
mvMatches12.push_back(make_pair(i,vMatches12[i]));
//标记参考帧1中的这个特征点有匹配关系
mvbMatched1[i]=true;
}
else
//标记参考帧1中的这个特征点没有匹配关系
mvbMatched1[i]=false;
}
在所有匹配的特征点中随机取出8组点，求解单应矩阵与基础矩阵
（1）获得RANSAC算法中，每次进行计算的8组点的索引
mvSets：迭代次数索引—>每次迭代时特征点对的索引
for(int it=0; it<mMaxIterations; it++)
{
//迭代开始的时候，所有的点都是可用的
vAvailableIndices = vAllIndices;
// Select a minimum set
//选择最小的数据样本集，使用八点法求，所以这里就循环了八次
for(size_t j=0; j<8; j++)
{
// 随机产生一对点的id,范围从0到N-1
int randi = DUtils::Random::RandomInt(0,vAvailableIndices.size()-1);
// idx表示哪一个索引对应的特征点对被选中
int idx = vAvailableIndices[randi];
//将本次迭代这个选中的第j个特征点对的索引添加到mvSets中
mvSets[it][j] = idx;
// 由于这对点在本次迭代中已经被使用了,所以我们为了避免再次抽到这个点,就在"点的可选列表"中,
// 将这个点原来所在的位置用vector最后一个元素的信息覆盖,并且删除尾部的元素
// 这样就相当于将这个点的信息从"点的可用列表"中直接删除了
vAvailableIndices[randi] = vAvailableIndices.back();
vAvailableIndices.pop_back();
}//依次提取出8个特征点对
}//迭代mMaxIterations次，选取各自迭代时需要用到的最小数据集
（2）开两个线程，分别计算基础矩阵和单应矩阵
// 构造线程来计算H矩阵及其得分
// thread方法比较特殊，在传递引用的时候，外层需要用ref来进行引用传递，否则就是浅拷贝
thread threadH(&Initializer::FindHomography, //该线程的主函数
this, //由于主函数为类的成员函数，所以第一个参数就应该是当前对象的this指针
ref(vbMatchesInliersH), //输出，特征点对的Inlier标记
ref(SH), //输出，计算的单应矩阵的RANSAC评分
ref(H)); //输出，计算的单应矩阵结果
// 计算fundamental matrix并打分，参数定义和H是一样的，这里不再赘述
thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));
// Wait until both threads have finished
//等待两个计算线程结束
threadH.join();
threadF.join();
根据评分选择F/ H，恢复R，t
更倾向于选择单应矩阵。因为单应矩阵对低视差的容忍程度比基础矩阵高。
float RH = SH/(SH+SF); //RH=Ratio of Homography
// Try to reconstruct from homography or fundamental depending on the ratio (0.40-0.45)
// 注意这里更倾向于用H矩阵恢复位姿。如果单应矩阵的评分占比达到了0.4以上,则从单应矩阵恢复运动,否则从基础矩阵恢复运动
if(RH>0.40)
//更偏向于平面，此时从单应矩阵恢复，函数ReconstructH返回bool型结果
return ReconstructH(vbMatchesInliersH, //输入，匹配成功的特征点对Inliers标记
H, //输入，前面RANSAC计算后的单应矩阵
mK, //输入，相机的内参数矩阵
R21,t21, //输出，计算出来的相机从参考帧1到当前帧2所发生的旋转和位移变换
vP3D, //特征点对经过三角测量之后的空间坐标，也就是地图点
vbTriangulated, //特征点对是否成功三角化的标记
1.0, //这个对应的形参为minParallax，即认为某对特征点的三角化测量中，认为其测量有效时
//需要满足的最小视差角（如果视差角过小则会引起非常大的观测误差）,单位是角度
50); //为了进行运动恢复，所需要的最少的三角化测量成功的点个数
else //if(pF_HF>0.6)
// 更偏向于非平面，从基础矩阵恢复
return ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,1.0,50);
从F中恢复R,t
根据F矩阵获得E矩阵
cv::Mat E21 = K.t()*F21*K;
分解E矩阵，获得4组解
DecomposeE(E21,R1,R2,t);
cv::Mat t1=t;
cv::Mat t2=-t;
选择最佳的R，t解
若某一组合使恢复得到的3D点位于相机正前方的数量最多，那么该组合就是最佳组合。**E矩阵分解有4中可能，H矩阵分解有8种可能。**在这个过程中，会进行三角化。
（1）选出最合适的R，t解（内点在相机的前面，有足够的视角）
（2）4组解中，如果最优的R，t解不够突出，就放弃。
int nGood1 = CheckRT(R1,t1, //当前组解
mvKeys1,mvKeys2, //参考帧和当前帧中的特征点
mvMatches12, vbMatchesInliers, //特征点的匹配关系和Inliers标记
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);
保存分解结果
包括保存最佳R，t、三角化后的点的坐标等。
三角化
void Initializer::Triangulate(
const cv::KeyPoint &kp1, //特征点, in reference frame
const cv::KeyPoint &kp2, //特征点, in current frame
const cv::Mat &P1, //投影矩阵P1（K*T）
const cv::Mat &P2, //投影矩阵P2
cv::Mat &x3D) //三维点
进行SVD分解，D的列向量即为AX=0的解X。随后对X进行归一化处理。
//构造参数矩阵A
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);
//奇异值分解的结果
cv::Mat u,w,vt;
//对系数矩阵A进行奇异值分解
cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
//根据前面的结论，奇异值分解右矩阵的最后一行其实就是解，原理类似于前面的求最小二乘解，四个未知数四个方程正好正定
//别忘了我们更习惯用列向量来表示一个点的空间坐标
x3D = vt.row(3).t();
//为了符合其次坐标的形式，使最后一维为1
x3D = x3D.rowRange(0,3)/x3D.at<float>(3);
三角化
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)
遍历特征点对，进行三角化
for(size_t i=0, iend=vMatches12.size();i<iend;i++)
{
// 跳过outliers
if(!vbMatchesInliers[i])
continue;
// Step 2 获取特征点对，调用Triangulate() 函数进行三角化，得到三角化测量之后的3D点坐标
// 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); //输出，三角化测量之后特征点的空间坐标
.....................
}
检查三角化后的路标点
（1）三角化后的路标点的X，Y，Z不能无限大。
if(!isfinite(p3dC1.at<float>(0)) || !isfinite(p3dC1.at<float>(1)) || !isfinite(p3dC1.at<float>(2)))
{
//其实这里就算是不这样写也没问题，因为默认的匹配点对就不是good点
vbGood[vMatches12[i].first]=false;
//继续对下一对匹配点的处理
continue;
}
（2）Z大于0，且视差不能太小
//得到向量PO1
cv::Mat normal1 = p3dC1 - O1;
//求取模长，其实就是距离
float dist1 = cv::norm(normal1);
//同理构造向量PO2
cv::Mat normal2 = p3dC1 - O2;
//求模长
float dist2 = cv::norm(normal2);
//根据公式：a.*b=|a||b|cos_theta 可以推导出来下面的式子
float cosParallax = normal1.dot(normal2)/(dist1*dist2);
// Check depth in front of first camera (only if enough parallax, as "infinite" points can easily go to negative depth)
// 如果深度值为负值，为非法三维点跳过该匹配点对
// ?视差比较小时，重投影误差比较大。这里0.99998 对应的角度为0.36°,这里不应该是 cosParallax>0.99998 吗？
// ?因为后面判断vbGood 点时的条件也是 cosParallax<0.99998
// !可能导致初始化不稳定
if(p3dC1.at<float>(2)<=0 && cosParallax<0.99998)
continue;
（3）路标点投影到图像中，重投影误差不能太大
float im1x, im1y;
//这个使能空间点的z坐标的倒数
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;
// Check reprojection error in second image
// 计算3D点在第二个图像上的投影误差，计算过程和第一个图像类似
float im2x, im2y;
// 注意这里的p3dC2已经是第二个相机坐标系下的三维点了
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;
取出一个较小的视差角
在分解F矩阵获得R、t，随后进行三角化之后会对视差角进行判断。如果视差角太小，说明这次平移很小，ReconstructF()函数直接返回失败。
在这里插入代码片
// Step 7 得到3D点中较大的视差角，并且转换成为角度制表示
if(nGood>0)
{
// 从小到大排序，注意vCosParallax值越大，视差越小
sort(vCosParallax.begin(),vCosParallax.end());
// !排序后并没有取最小的视差角，而是取一个较小的视差角
// 作者的做法：如果经过检验过后的有效3D点小于50个，那么就取最后那个最小的视差角(cos值最大)
// 如果大于50个，就取排名第50个的较小的视差角即可，为了避免3D点太多时出现太小的视差角
size_t idx = min(50,int(vCosParallax.size()-1));
//将这个选中的角弧度制转换为角度制
parallax = acos(vCosParallax[idx])*180/CV_PI;
}
SVD分解E矩阵，获得R,t
// |0 -1 0|
// E = U Sigma V' let W = |1 0 0|
// |0 0 1|
// 得到4个解 E = [R|t]
// R1 = UWV' R2 = UW'V' t1 = U3 t2 = -U3
void Initializer::DecomposeE(const cv::Mat &E, cv::Mat &R1, cv::Mat &R2, cv::Mat &t)
{
// 对本质矩阵进行奇异值分解
//准备存储对本质矩阵进行奇异值分解的结果
cv::Mat u,w,vt;
//对本质矩阵进行奇异值分解
cv::SVD::compute(E,w,u,vt);
// 左奇异值矩阵U的最后一列就是t，对其进行归一化
u.col(2).copyTo(t);
t=t/cv::norm(t);
// 构造一个绕Z轴旋转pi/2的旋转矩阵W，按照下式组合得到旋转矩阵 R1 = u*W*vt
//计算完成后要检查一下旋转矩阵行列式的数值，使其满足行列式为1的约束
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;
//旋转矩阵有行列式为+1的约束，所以如果算出来为负值，需要取反
if(cv::determinant(R1)<0)
R1=-R1;
// 同理将矩阵W取转置来按照相同的公式计算旋转矩阵R2 = u*W.t()*vt
R2 = u*W.t()*vt;
//旋转矩阵有行列式为1的约束
if(cv::determinant(R2)<0)
R2=-R2;
}
582
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
