/**

 * 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