ORB-SLAM（三）地图初始化

单目SLAM地图初始化的目标是构建初始的三维点云。由于不能仅仅从单帧得到深度信息，因此需要从图像序列中选取两帧以上的图像，估计摄像机姿态并重建出初始的三维点云。

ORB-SLAM中提到，地图初始化常见的方法有三种。

方法一

追踪一个已知物体。单帧图像的每一个点都对应于空间的一条射线。通过不同角度不同位置扫描同一个物体，期望能够将三维点的不确定性缩小到可接受的范围。

方法二

基于假设空间存在一个平面物体，选取两帧不同位置的图像，通过计算Homography来估计位姿。这类方法在视差较小或者平面上的点靠近某个主点时效果不好。

参考文献Faugeras et al, Motion and structure from motion in a piecewise planar environment. International Journal of Pattern Recognition and Artificial Intelligence, 1988.

方法三

根据两帧之间的特征点匹配计算Fundamental matrix，进一步估计位姿。这种方法要求存在不共面的特征点。

参考文献Hartley, Richard, and Andrew Zisserman. Multiple view geometry in computer vision. Cambridge university press, 2003.

ORB-SLAM的作者提出了一种基于统计的模型选择方法。该方法优先选择第三种方法，并期望在场景退化情形下自动选择第二种方法。如果选取的两帧不满足要求，放弃这两帧并重新初始化。

第一步

提取特征点并匹配，若匹配点足够多，尝试第二步。

第二步

利用匹配的特征点分别计算方法二和方法三。

对每个模型，首先归一化所有的特征点。然后，在每步迭代中，

1. 根据特征点对计算homography或者fundamental matrix。Homography的计算方法为normalized DLT，fundamental matrix的计算方法为normalized 8 points。

2. 计算每个点对的symmetric transfer errors，和卡方分布的对应值比较，由此判定该点是否为内点。累计内点的总得分。

模型选择方法参考4.7.1 RANSAC in Multiple View Geometry in Computer Vision。

3. 比较本次得分和历史得分，取最高分并记录相应参数。

第三步

根据一定的准则选择模型。用SH表示homography的得分，SF表示fundamental matrix的得分。如果SH / (SH + SF) > 0.4，那么选择homography，反之选择fundamental matrix。

第四步

根据选择的模型计算位姿。参考方法二和方法三。

第五步

Full bundle adjustment。

由于ORB-SLAM对初始化的要求较高，因此初始化时可以选择一个特征丰富的场景，移动摄像机给它提供足够的视差。另外，由于坐标系会附着在初始化成功的那帧图像的位置，因此每次初始化不能保证在同一个位置。

下面结合源代码进一步说明一下。

 /**

 * This file is part of ORB-SLAM2.

 *

 * Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)

 * For more information see <https://github.com/raulmur/ORB_SLAM2>

 *

 * ORB-SLAM2 is free software: you can redistribute it and/or modify

 * it under the terms of the GNU General Public License as published by

 * the Free Software Foundation, either version 3 of the License, or

 * (at your option) any later version.

 *

 * ORB-SLAM2 is distributed in the hope that it will be useful,

 * but WITHOUT ANY WARRANTY; without even the implied warranty of

 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the

 * GNU General Public License for more details.

 *

 * You should have received a copy of the GNU General Public License

 * along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.

 */

 #include "Initializer.h"

 #include "Thirdparty/DBoW2/DUtils/Random.h"

 #include "Optimizer.h"

 #include "ORBmatcher.h"

 #include<thread>

 namespace ORB_SLAM2 {

 Initializer::Initializer(const Frame &ReferenceFrame, float sigma, int iterations) {

   mK = ReferenceFrame.mK.clone();

   mvKeys1 = ReferenceFrame.mvKeysUn;

   mSigma = sigma;

   mSigma2 = sigma * sigma;

   mMaxIterations = iterations;

 }

 bool Initializer::Initialize(const Frame &CurrentFrame, const vector<int> &vMatches12, cv::Mat &R21, cv::Mat &t21,

                              vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated) {

   // Fill structures with current keypoints and matches with reference frame

   // Reference Frame: 1, Current Frame: 2

   // Frame 2 特征点

   mvKeys2 = CurrentFrame.mvKeysUn;

   mvMatches12.clear();

   mvMatches12.reserve(mvKeys2.size());

   mvbMatched1.resize(mvKeys1.size());

   // 组织特征点对

   for (size_t i = , iend = vMatches12.size(); i < iend; i++) {

     if (vMatches12[i] >= ) {

       mvMatches12.push_back(make_pair(i, vMatches12[i]));

       mvbMatched1[i] = true;

     } else

       mvbMatched1[i] = false;

   }

   const int N = mvMatches12.size();

   // Indices for minimum set selection

   vector<size_t> vAllIndices;

   vAllIndices.reserve(N);

   vector<size_t> vAvailableIndices;

   for (int i = ; i < N; i++) {

     vAllIndices.push_back(i);

   }

   // Generate sets of 8 points for each RANSAC iteration

   // 为每个iteration选取8个随机的index（在FindHomography和FindFundamental被调用）

   mvSets = vector< vector<size_t> >(mMaxIterations, vector<size_t>(, ));

   DUtils::Random::SeedRandOnce();

   for (int it = ; it < mMaxIterations; it++) {

     vAvailableIndices = vAllIndices;

     // Select a minimum set

     for (size_t j = ; j < ; j++) {

       int randi = DUtils::Random::RandomInt(, vAvailableIndices.size() - );

       int idx = vAvailableIndices[randi];

       mvSets[it][j] = idx;

       vAvailableIndices[randi] = vAvailableIndices.back();

       vAvailableIndices.pop_back();

     }

   }

   // Launch threads to compute in parallel a fundamental matrix and a homography

   // 调起多线程分别用于计算fundamental matrix和homography

   vector<bool> vbMatchesInliersH, vbMatchesInliersF;

   float SH, SF;

   cv::Mat H, F;

   // 计算homograpy并打分

   thread threadH(&Initializer::FindHomography, this, ref(vbMatchesInliersH), ref(SH), ref(H));

   // 计算fundamental matrix并打分

   thread threadF(&Initializer::FindFundamental, this, ref(vbMatchesInliersF), ref(SF), ref(F));

   // Wait until both threads have finished

   threadH.join();

   threadF.join();

   // Compute ratio of scores

   // 计算比例，选取某个模型

   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, );

   else //if(pF_HF>0.6)

     return ReconstructF(vbMatchesInliersF, F, mK, R21, t21, vP3D, vbTriangulated, 1.0, );

   return false;

 }

 void Initializer::FindHomography(vector<bool> &vbMatchesInliers, float &score, cv::Mat &H21) {

   // Number of putative matches

   const int N = mvMatches12.size();

   // Normalize coordinates

   vector<cv::Point2f> vPn1, vPn2;

   cv::Mat T1, T2;

   Normalize(mvKeys1, vPn1, T1);

   Normalize(mvKeys2, vPn2, T2);

   cv::Mat T2inv = T2.inv();

   // Best Results variables

   score = 0.0;

   vbMatchesInliers = vector<bool>(N, false);

   // Iteration variables

   vector<cv::Point2f> vPn1i();

   vector<cv::Point2f> vPn2i();

   cv::Mat H21i, H12i;

   vector<bool> vbCurrentInliers(N, false);

   float currentScore;

   // Perform all RANSAC iterations and save the solution with highest score

   for (int it = ; it < mMaxIterations; it++) {

     // Select a minimum set

     for (size_t j = ; j < ; j++) {

       int idx = mvSets[it][j];

       vPn1i[j] = vPn1[mvMatches12[idx].first];

       vPn2i[j] = vPn2[mvMatches12[idx].second];

     }

     cv::Mat Hn = ComputeH21(vPn1i, vPn2i);

     H21i = T2inv * Hn * T1;

     H12i = H21i.inv();

     currentScore = CheckHomography(H21i, H12i, vbCurrentInliers, mSigma);

     if (currentScore > score) {

       H21 = H21i.clone();

       vbMatchesInliers = vbCurrentInliers;

       score = currentScore;

     }

   }

 }

 void Initializer::FindFundamental(vector<bool> &vbMatchesInliers, float &score, cv::Mat &F21) {

   // 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();

   // Best Results variables

   score = 0.0;

   vbMatchesInliers = vector<bool>(N, false);

   // Iteration variables

   vector<cv::Point2f> vPn1i();

   vector<cv::Point2f> vPn2i();

   cv::Mat F21i;

   vector<bool> vbCurrentInliers(N, false);

   float currentScore;

   // Perform all RANSAC iterations and save the solution with highest score

   for (int it = ; it < mMaxIterations; it++) {

     // Select a minimum set

     for (int j = ; j < ; j++) {

       int idx = mvSets[it][j];

       vPn1i[j] = vPn1[mvMatches12[idx].first];

       vPn2i[j] = vPn2[mvMatches12[idx].second];

     }

     cv::Mat Fn = ComputeF21(vPn1i, vPn2i);

     F21i = T2t * Fn * T1;

     currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma);

     if (currentScore > score) {

       F21 = F21i.clone();

       vbMatchesInliers = vbCurrentInliers;

       score = currentScore;

     }

   }

 }

 // 从特征点匹配求homography（normalized DLT）

 // Algorithm 4.2 in Multiple View Geometry in Computer Vision.

 cv::Mat Initializer::ComputeH21(const vector<cv::Point2f> &vP1, const vector<cv::Point2f> &vP2) {

   const int N = vP1.size();

   cv::Mat A( * N, , CV_32F);

   for (int i = ; 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>( * i, ) = 0.0;

     A.at<float>( * i, ) = 0.0;

     A.at<float>( * i, ) = 0.0;

     A.at<float>( * i, ) = -u1;

     A.at<float>( * i, ) = -v1;

     A.at<float>( * i, ) = -;

     A.at<float>( * i, ) = v2 * u1;

     A.at<float>( * i, ) = v2 * v1;

     A.at<float>( * i, ) = v2;

     A.at<float>( * i + , ) = u1;

     A.at<float>( * i + , ) = v1;

     A.at<float>( * i + , ) = ;

     A.at<float>( * i + , ) = 0.0;

     A.at<float>( * i + , ) = 0.0;

     A.at<float>( * i + , ) = 0.0;

     A.at<float>( * i + , ) = -u2 * u1;

     A.at<float>( * i + , ) = -u2 * v1;

     A.at<float>( * i + , ) = -u2;

   }

   cv::Mat u, w, vt;

   cv::SVDecomp(A, w, u, vt, cv::SVD::MODIFY_A | cv::SVD::FULL_UV);

   return vt.row().reshape(, );

 }

 // 从特征点匹配求fundamental matrix（8点法）

 // Algorithm 11.1 in Multiple View Geometry in Computer Vision.

 cv::Mat Initializer::ComputeF21(const vector<cv::Point2f> &vP1, const vector<cv::Point2f> &vP2) {

   const int N = vP1.size();

   cv::Mat A(N, , CV_32F);

   for (int i = ; 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>(i, ) = u2 * u1;

     A.at<float>(i, ) = u2 * v1;

     A.at<float>(i, ) = u2;

     A.at<float>(i, ) = v2 * u1;

     A.at<float>(i, ) = v2 * v1;

     A.at<float>(i, ) = v2;

     A.at<float>(i, ) = u1;

     A.at<float>(i, ) = v1;

     A.at<float>(i, ) = ;

   }

   cv::Mat u, w, vt;

   cv::SVDecomp(A, w, u, vt, cv::SVD::MODIFY_A | cv::SVD::FULL_UV);

   cv::Mat Fpre = vt.row().reshape(, );

   cv::SVDecomp(Fpre, w, u, vt, cv::SVD::MODIFY_A | cv::SVD::FULL_UV);

   w.at<float>() = ;

   return  u * cv::Mat::diag(w) * vt;

 }

 // 从homography计算symmetric transfer errors

 // IV. AUTOMATIC MAP INITIALIZATION （2） in Author's paper

 // symmetric transfer errors:

 //    4.2.2 Geometric distance in Multiple View Geometry in Computer Vision.

 // model selection

 //    4.7.1 RANSAC in Multiple View Geometry in Computer Vision

 float Initializer::CheckHomography(const cv::Mat &H21, const cv::Mat &H12, vector<bool> &vbMatchesInliers, float sigma) {

   const int N = mvMatches12.size();

   const float h11 = H21.at<float>(, );

   const float h12 = H21.at<float>(, );

   const float h13 = H21.at<float>(, );

   const float h21 = H21.at<float>(, );

   const float h22 = H21.at<float>(, );

   const float h23 = H21.at<float>(, );

   const float h31 = H21.at<float>(, );

   const float h32 = H21.at<float>(, );

   const float h33 = H21.at<float>(, );

   const float h11inv = H12.at<float>(, );

   const float h12inv = H12.at<float>(, );

   const float h13inv = H12.at<float>(, );

   const float h21inv = H12.at<float>(, );

   const float h22inv = H12.at<float>(, );

   const float h23inv = H12.at<float>(, );

   const float h31inv = H12.at<float>(, );

   const float h32inv = H12.at<float>(, );

   const float h33inv = H12.at<float>(, );

   vbMatchesInliers.resize(N);

   float score = ;

   // 来源于卡方分布

   const float th = 5.991;

   const float invSigmaSquare = 1.0 / (sigma * sigma);

   for (int i = ; 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;

     // Reprojection error in first image

     // x2in1 = H12*x2

     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;

     if (chiSquare1 > th)

       bIn = false;

     else

       score += th - chiSquare1;

     // Reprojection error in second image

     // x1in2 = H21*x1

     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;

     if (bIn)

       vbMatchesInliers[i] = true;

     else

       vbMatchesInliers[i] = false;

   }

   return score;

 }

 // 从fundamental matrix计算symmetric transfer errors

 // IV. AUTOMATIC MAP INITIALIZATION （2） in Author's paper

 // symmetric transfer errors:

 //    4.2.2 Geometric distance in Multiple View Geometry in Computer Vision.

 // model selection

 //    4.7.1 RANSAC in Multiple View Geometry in Computer Vision

 float Initializer::CheckFundamental(const cv::Mat &F21, vector<bool> &vbMatchesInliers, float sigma) {

   const int N = mvMatches12.size();

   const float f11 = F21.at<float>(, );

   const float f12 = F21.at<float>(, );

   const float f13 = F21.at<float>(, );

   const float f21 = F21.at<float>(, );

   const float f22 = F21.at<float>(, );

   const float f23 = F21.at<float>(, );

   const float f31 = F21.at<float>(, );

   const float f32 = F21.at<float>(, );

   const float f33 = F21.at<float>(, );

   vbMatchesInliers.resize(N);

   float score = ;

   const float th = 3.841;

   const float thScore = 5.991;

   const float invSigmaSquare = 1.0 / (sigma * sigma);

   for (int i = ; 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;

     // Reprojection error in second image

     // l2=F21x1=(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;

     const float num2 = a2 * u2 + b2 * v2 + c2;

     const float squareDist1 = num2 * num2 / (a2 * a2 + b2 * b2);

     const float chiSquare1 = squareDist1 * invSigmaSquare;

     if (chiSquare1 > th)

       bIn = false;

     else

       score += thScore - chiSquare1;

     // Reprojection error in second image

     // l1 =x2tF21=(a1,b1,c1)

     const float a1 = f11 * u2 + f21 * v2 + f31;

     const float b1 = f12 * u2 + f22 * v2 + f32;

     const float c1 = f13 * u2 + f23 * v2 + f33;

     const float num1 = a1 * u1 + b1 * v1 + c1;

     const float squareDist2 = num1 * num1 / (a1 * a1 + b1 * b1);

     const float chiSquare2 = squareDist2 * invSigmaSquare;

     if (chiSquare2 > th)

       bIn = false;

     else

       score += thScore - chiSquare2;

     if (bIn)

       vbMatchesInliers[i] = true;

     else

       vbMatchesInliers[i] = false;

   }

   return score;

 }

 // 从F恢复P

 // 参考文献：

 // Result 9.19 in Multiple View Geometry in Computer Vision

 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) {

   int N = ;

   for (size_t i = , iend = vbMatchesInliers.size() ; i < iend; i++)

     if (vbMatchesInliers[i])

       N++;

   // Compute Essential Matrix from Fundamental Matrix

   cv::Mat E21 = K.t() * F21 * K;

   cv::Mat R1, R2, t;

   // Recover the 4 motion hypotheses

   DecomposeE(E21, R1, R2, t);

   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);

   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 = ;

   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 > ) {

     return false;

   }

   // 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;

 }

 // 从H恢复P

 // 参考文献：

 // Faugeras et al, Motion and structure from motion in a piecewise planar environment. International Journal of Pattern Recognition and Artificial Intelligence, 1988.

 bool Initializer::ReconstructH(vector<bool> &vbMatchesInliers, cv::Mat &H21, cv::Mat &K,

                                cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated, float minParallax, int minTriangulated) {

   int N = ;

   for (size_t i = , iend = vbMatchesInliers.size() ; i < iend; i++)

     if (vbMatchesInliers[i])

       N++;

   // 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

   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();

   float s = cv::determinant(U) * cv::determinant(Vt);

   float d1 = w.at<float>();

   float d2 = w.at<float>();

   float d3 = w.at<float>();

   if (d1 / d2 < 1.00001 || d2 / d3 < 1.00001) {

     return false;

   }

   vector<cv::Mat> vR, vt, vn;

   vR.reserve();

   vt.reserve();

   vn.reserve();

   //n'=[x1 0 x3] 4 posibilities e1=e3=1, e1=1 e3=-1, e1=-1 e3=1, e1=e3=-1

   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};

   //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};

   for (int i = ; i < ; i++) {

     cv::Mat Rp = cv::Mat::eye(, , CV_32F);

     Rp.at<float>(, ) = ctheta;

     Rp.at<float>(, ) = -stheta[i];

     Rp.at<float>(, ) = stheta[i];

     Rp.at<float>(, ) = ctheta;

     cv::Mat R = s * U * Rp * Vt;

     vR.push_back(R);

     cv::Mat tp(, , CV_32F);

     tp.at<float>() = x1[i];

     tp.at<float>() = ;

     tp.at<float>() = -x3[i];

     tp *= d1 - d3;

     cv::Mat t = U * tp;

     vt.push_back(t / cv::norm(t));

     cv::Mat np(, , CV_32F);

     np.at<float>() = x1[i];

     np.at<float>() = ;

     np.at<float>() = x3[i];

     cv::Mat n = V * np;

     if (n.at<float>() < )

       n = -n;

     vn.push_back(n);

   }

   //case d'=-d2

   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};

   for (int i = ; i < ; i++) {

     cv::Mat Rp = cv::Mat::eye(, , CV_32F);

     Rp.at<float>(, ) = cphi;

     Rp.at<float>(, ) = sphi[i];

     Rp.at<float>(, ) = -;

     Rp.at<float>(, ) = sphi[i];

     Rp.at<float>(, ) = -cphi;

     cv::Mat R = s * U * Rp * Vt;

     vR.push_back(R);

     cv::Mat tp(, , CV_32F);

     tp.at<float>() = x1[i];

     tp.at<float>() = ;

     tp.at<float>() = x3[i];

     tp *= d1 + d3;

     cv::Mat t = U * tp;

     vt.push_back(t / cv::norm(t));

     cv::Mat np(, , CV_32F);

     np.at<float>() = x1[i];

     np.at<float>() = ;

     np.at<float>() = x3[i];

     cv::Mat n = V * np;

     if (n.at<float>() < )

       n = -n;

     vn.push_back(n);

   }

   int bestGood = ;

   int secondBestGood = ;

   int bestSolutionIdx = -;

   float bestParallax = -;

   vector<cv::Point3f> bestP3D;

   vector<bool> bestTriangulated;

   // Instead of applying the visibility constraints proposed in the Faugeras' paper (which could fail for points seen with low parallax)

   // We reconstruct all hypotheses and check in terms of triangulated points and parallax

   for (size_t i = ; i < ; 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;

     }

   }

   if (secondBestGood < 0.75 * bestGood && bestParallax >= minParallax && bestGood > minTriangulated && bestGood > 0.9 * N) {

     vR[bestSolutionIdx].copyTo(R21);

     vt[bestSolutionIdx].copyTo(t21);

     vP3D = bestP3D;

     vbTriangulated = bestTriangulated;

     return true;

   }

   return false;

 }

 void Initializer::Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D) {

   cv::Mat A(, , CV_32F);

   A.row() = kp1.pt.x * P1.row() - P1.row();

   A.row() = kp1.pt.y * P1.row() - P1.row();

   A.row() = kp2.pt.x * P2.row() - P2.row();

   A.row() = kp2.pt.y * P2.row() - P2.row();

   cv::Mat u, w, vt;

   cv::SVD::compute(A, w, u, vt, cv::SVD::MODIFY_A | cv::SVD::FULL_UV);

   x3D = vt.row().t();

   x3D = x3D.rowRange(, ) / x3D.at<float>();

 }

 // 归一化特征点到同一尺度（作为normalize DLT的输入）

 void Initializer::Normalize(const vector<cv::KeyPoint> &vKeys, vector<cv::Point2f> &vNormalizedPoints, cv::Mat &T) {

   float meanX = ;

   float meanY = ;

   const int N = vKeys.size();

   vNormalizedPoints.resize(N);

   for (int i = ; i < N; i++) {

     meanX += vKeys[i].pt.x;

     meanY += vKeys[i].pt.y;

   }

   meanX = meanX / N;

   meanY = meanY / N;

   float meanDevX = ;

   float meanDevY = ;

   for (int i = ; 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;

   for (int i = ; i < N; i++) {

     vNormalizedPoints[i].x = vNormalizedPoints[i].x * sX;

     vNormalizedPoints[i].y = vNormalizedPoints[i].y * sY;

   }

   T = cv::Mat::eye(, , CV_32F);

   T.at<float>(, ) = sX;

   T.at<float>(, ) = sY;

   T.at<float>(, ) = -meanX * sX;

   T.at<float>(, ) = -meanY * sY;

 }

 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 &parallax) {

   // Calibration parameters

   const float fx = K.at<float>(, );

   const float fy = K.at<float>(, );

   const float cx = K.at<float>(, );

   const float cy = K.at<float>(, );

   vbGood = vector<bool>(vKeys1.size(), false);

   vP3D.resize(vKeys1.size());

   vector<float> vCosParallax;

   vCosParallax.reserve(vKeys1.size());

   // Camera 1 Projection Matrix K[I|0]

   cv::Mat P1(, , CV_32F, cv::Scalar());

   K.copyTo(P1.rowRange(, ).colRange(, ));

   cv::Mat O1 = cv::Mat::zeros(, , CV_32F);

   // Camera 2 Projection Matrix K[R|t]

   cv::Mat P2(, , CV_32F);

   R.copyTo(P2.rowRange(, ).colRange(, ));

   t.copyTo(P2.rowRange(, ).col());

   P2 = K * P2;

   cv::Mat O2 = -R.t() * t;

   int nGood = ;

   for (size_t i = , iend = vMatches12.size(); i < iend; i++) {

     if (!vbMatchesInliers[i])

       continue;

     const cv::KeyPoint &kp1 = vKeys1[vMatches12[i].first];

     const cv::KeyPoint &kp2 = vKeys2[vMatches12[i].second];

     cv::Mat p3dC1;

     Triangulate(kp1, kp2, P1, P2, p3dC1);

     if (!isfinite(p3dC1.at<float>()) || !isfinite(p3dC1.at<float>()) || !isfinite(p3dC1.at<float>())) {

       vbGood[vMatches12[i].first] = false;

       continue;

     }

     // 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);

     // Check depth in front of first camera (only if enough parallax, as "infinite" points can easily go to negative depth)

     if (p3dC1.at<float>() <=  && cosParallax < 0.99998)

       continue;

     // Check depth in front of second camera (only if enough parallax, as "infinite" points can easily go to negative depth)

     cv::Mat p3dC2 = R * p3dC1 + t;

     if (p3dC2.at<float>() <=  && cosParallax < 0.99998)

       continue;

     // Check reprojection error in first image

     float im1x, im1y;

     float invZ1 = 1.0 / p3dC1.at<float>();

     im1x = fx * p3dC1.at<float>() * invZ1 + cx;

     im1y = fy * p3dC1.at<float>() * 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

     float im2x, im2y;

     float invZ2 = 1.0 / p3dC2.at<float>();

     im2x = fx * p3dC2.at<float>() * invZ2 + cx;

     im2y = fy * p3dC2.at<float>() * invZ2 + cy;

     float squareError2 = (im2x - kp2.pt.x) * (im2x - kp2.pt.x) + (im2y - kp2.pt.y) * (im2y - kp2.pt.y);

     if (squareError2 > th2)

       continue;

     vCosParallax.push_back(cosParallax);

     vP3D[vMatches12[i].first] = cv::Point3f(p3dC1.at<float>(), p3dC1.at<float>(), p3dC1.at<float>());

     nGood++;

     if (cosParallax < 0.99998)

       vbGood[vMatches12[i].first] = true;

   }

   if (nGood > ) {

     sort(vCosParallax.begin(), vCosParallax.end());

     size_t idx = min(, int(vCosParallax.size() - ));

     parallax = acos(vCosParallax[idx]) *  / CV_PI;

   } else

     parallax = ;

   return nGood;

 }

 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.col().copyTo(t);

   t = t / cv::norm(t);

   cv::Mat W(, , CV_32F, cv::Scalar());

   W.at<float>(, ) = -;

   W.at<float>(, ) = ;

   W.at<float>(, ) = ;

   R1 = u * W * vt;

   if (cv::determinant(R1) < )

     R1 = -R1;

   R2 = u * W.t() * vt;

   if (cv::determinant(R2) < )

     R2 = -R2;

 }

 } //namespace ORB_SLAM

该系列的其它文章：

ORB-SLAM（三）地图初始化的更多相关文章

arcgis api for js入门开发系列三地图工具栏(含源代码)
上一篇实现了demo的地图加载展示,在上篇实现的基础上,新增了地图工具栏以及通用地图控件功能,比如地图框选缩放.地图漫游.清空.量算工具.地图导航控件.地图比例尺控件.地图鹰眼图等等,总共分为5个部分 ...
arcgis api 3.x for js 入门开发系列三地图工具栏（附源码下载）
前言关于本篇功能实现用到的 api 涉及类看不懂的,请参照 esri 官网的 arcgis api 3.x for js:esri 官网 api,里面详细的介绍 arcgis api 3.x 各个类 ...
acgis地图初始化并根据经纬度进行标注
根据项目要求,需要对指定的位置进行标注. 1.初始化地图服务 2.根据地图服务接口信息进行标注 3.根据经纬度信息进行标注展示接口截图: 以下代码并不能直接粘贴跑起来,因为代码所涉及的文件地址都是内 ...
关于java中三种初始化块的执行顺序
许多小伙伴对于java中的三种初始化块的执行顺序一直感到头疼,接下来我们就来分析一下这三种初始化块到底是怎么运行的.有些公司也会将这个问题作为笔试题目. 下面通过一段代码来看看创建对象时这么初始化块是 ...
leaflet-webpack 入门开发系列三地图分屏对比(附源码下载)
前言 leaflet-webpack 入门开发系列环境知识点了解: node 安装包下载webpack 打包管理工具需要依赖 node 环境,所以 node 安装包必须安装,上面链接是官网下载地址 w ...
DirectX11笔记(三)--Direct3D初始化代码
原文:DirectX11笔记(三)--Direct3D初始化代码版权声明:本文为博主原创文章,未经博主允许不得转载. https://blog.csdn.net/u010333737/article ...
DirectX11笔记(三)--Direct3D初始化2
原文:DirectX11笔记(三)--Direct3D初始化2 版权声明:本文为博主原创文章,未经博主允许不得转载. https://blog.csdn.net/u010333737/article/ ...
第5章节 BJROBOT SLAM 构建地图
第五章节 BJROBOT SLAM 构建地图建地图前说明:请确保你的小车已经校正好 IMU.角速度.线速度,虚拟机配置好 ROS 网络的前提进行,否则会造成构建地图无边界.虚拟机端无法正常收到小 ...
Day008 三种初始化及内存分析
三种初始化和内存分析 Java内存分析: 堆: 存放new的对象和数组. 可以被所有的线程共享,不会存放别的对象引用. 栈: 存放基本变量类型(会包含这个基本类型的具体数值). 引用对象的变量(会存放 ...

随机推荐

Windows on Device 项目实践 3 - 火焰报警器制作
在前两篇<Windows on Device 项目实践 1 - PWM调光灯制作>和<Windows on Device 项目实践 2 - 感光灯制作>中,我们学习了如何利用I ...
将String转化成Stream，将Stream转换成String
using System;using System.IO;using System.Text;namespace CSharpConvertString2Stream{ class Progr ...
mysqldump: Got error: 1142: SELECT, LOCK TABLES command denied to user 'root'@'localhost' for table 'accounts' when using LOCK TABLES
AutoMySQLBackup备份时,出现mysqldump: Got error: 1142: SELECT, LOCK TABLES command denied to user 'root'@' ...
Oracle物理体系结构
一.ORACLE 物理体系结构原理结构图各部分解释: PGA: 私有内存区,仅供当前发起用户使用. 三个作用用户登录后的session信息会保存在PGA. 执行排序,如果内存不够,oracle会 ...
.NET应用程序调试—原理、工具、方法
阅读目录: 1.背景介绍 2.基本原理(Windows调试工具箱..NET调试扩展SOS.DLL.SOSEX.DLL) 2.1.Windows调试工具箱 2.2..NET调试扩展包,SOS.DLL.S ...
DIV+CSS规范命名大全
网页制作中规范使用DIV+CSS命名规则,可以改善优化功效特别是团队合作时候可以提供合作制作效率,具体DIV CSS命名规则CSS命名大全内容篇. 常用DIV+CSS命名大全集合,即CSS命名规则 D ...
[原创]首次制作JQueryUI插件-Timeline时间轴
特点: 1. 支持多左右滚动,左右拖动. 2. 时间轴可上下两种显示方式. 3. 支持两种模式的平滑滚动/拖动. 4. 行压缩(后续版本此处可设置是否开启,上传的代码不带这个功能). 5. 支持hov ...
sql中毫秒数与格式化时间的转换
使用MYSQL自带的函数FROM_UNIXTIME(unix_timestamp,format). 如: SELECT FROM_UNIXTIME(1461201575895/1000,"% ...
Linux 目录相关命令(1)
1:分区分区是指逻辑分区,主分区最多四个,由硬盘结构决定 windows里A和B做软驱判定,C主分区判定 1.硬盘分为若干个等大的扇区每个扇区默认512字节,其中446字节用于启动信息,64字节用 ...
POJ 1325 Machine Schedule——S.B.S.
Machine Schedule Time Limit: 1000MS Memory Limit: 10000K Total Submissions: 13731 Accepted: 5873 ...

ORB-SLAM（三）地图初始化

ORB-SLAM（三）地图初始化的更多相关文章

随机推荐

热门专题