diff --git a/CMakeSettings.json b/CMakeSettings.json index 2e6e3f2e..8077c4b2 100644 --- a/CMakeSettings.json +++ b/CMakeSettings.json @@ -11,7 +11,7 @@ "buildCommandArgs": "", "ctestCommandArgs": "", "intelliSenseMode": "windows-msvc-x64", - "cmakeToolchain": "E:/vcpkg/scripts/buildsystems/vcpkg.cmake", + "cmakeToolchain": "C:/Users/mhmds/vcpkg/scripts/buildsystems/vcpkg.cmake", "variables": [ { "name": "MSVC_USE_STATIC_CRT", diff --git a/Thirdparty/g2o/g2o/core/optimizable_graph.cpp b/Thirdparty/g2o/g2o/core/optimizable_graph.cpp index 0a2e24cf..c38f3d5c 100644 --- a/Thirdparty/g2o/g2o/core/optimizable_graph.cpp +++ b/Thirdparty/g2o/g2o/core/optimizable_graph.cpp @@ -248,10 +248,7 @@ namespace g2o { bool OptimizableGraph::addEdge(HyperGraph::Edge *e_) { OptimizableGraph::Edge *e = dynamic_cast(e_); assert(e && "Edge does not inherit from OptimizableGraph::Edge"); - if (!e) - return false; - bool eresult = HyperGraph::addEdge(e); - if (!eresult) + if (!e || !HyperGraph::addEdge(e)) return false; e->_internalId = _nextEdgeId++; if (!e->resolveParameters()) { diff --git a/generalSettings.json b/generalSettings.json index c6c5b910..7a045b87 100644 --- a/generalSettings.json +++ b/generalSettings.json @@ -1,12 +1,12 @@ { - "VocabularyPath": "E:\\rbdlabRepo\\simulatorMapping\\Thirdparty\\DBow3\\orbvoc.dbow3", - "DroneYamlPathSlam": "E:\\rbdlabRepo\\simulatorMapping\\config\\tello_9F5EC2_640.yaml", - "offlineVideoTestPath": "C:\\Users\\tzuk\\Downloads\\mapping.avi", + "VocabularyPath": "C:\\Users\\mhmds\\simulatorMapping\\Thirdparty\\DBow3\\orbvoc.dbow3", + "DroneYamlPathSlam": "C:\\Users\\mhmds\\simulatorMapping\\config\\tello_9F5EC2_640.yaml", + "offlineVideoTestPath": "C:\\Users\\mhmds\\mapping.avi", "onlineVideoPath": "udp://0.0.0.0:11111?overrun_nonfatal=1&fifo_size=1000", "loadMap": false, "loadMapPath": "/home/tzuk/slamMaps/HaifaLab.bin", "saveMap": true, - "simulatorOutputDir": "E:\\simulatorMapping\\slamMaps", + "simulatorOutputDir": "C:\\Users\\mhmds\\simulatorMapping\\slamMaps", "modelTextureNameToAlignTo": "floor", "mapInputDir": "/home/tzuk/slamMaps/example_mapping11/", "getPointDataCsv": "/home/liam/example.csv", @@ -25,7 +25,7 @@ "rollRad": 2.87, "movingScale": 0.2, "rotateScale": 0.2, - "modelPath": "E:\\simulatorMapping\\TLV-data\\drones_lab.obj", + "modelPath": "C:\\Users\\mhmds\\simulatorMapping\\TLV-data\\drones_lab.obj", "pointSize": 10, "xOffset": 0.0696954, "yOffset": 0.232057, diff --git a/slam/include/Frame.h b/slam/include/Frame.h index 3a2c12b4..997ab478 100644 --- a/slam/include/Frame.h +++ b/slam/include/Frame.h @@ -204,6 +204,12 @@ namespace ORB_SLAM2 { static bool mbInitialComputations; + // Rotation, translation and camera center + cv::Mat mRcw; + cv::Mat mtcw; + cv::Mat mRwc; + cv::Mat mOw; //==mtwc + private: @@ -219,12 +225,6 @@ namespace ORB_SLAM2 { // Assign keypoints to the grid for speed up feature matching (called in the constructor). void AssignFeaturesToGrid(); - - // Rotation, translation and camera center - cv::Mat mRcw; - cv::Mat mtcw; - cv::Mat mRwc; - cv::Mat mOw; //==mtwc }; }// namespace ORB_SLAM diff --git a/slam/src/Frame.cc b/slam/src/Frame.cc index 5ad9ad80..5648211f 100644 --- a/slam/src/Frame.cc +++ b/slam/src/Frame.cc @@ -331,7 +331,7 @@ namespace ORB_SLAM2 { mRcw = mTcw.rowRange(0, 3).colRange(0, 3); mRwc = mRcw.t(); mtcw = mTcw.rowRange(0, 3).col(3); - mOw = -mRcw.t() * mtcw; + mOw = -mRwc * mtcw; } bool Frame::isInFrustum(MapPoint *pMP, float viewingCosLimit) { diff --git a/slam/src/MapPoint.cc b/slam/src/MapPoint.cc index c458fce0..73fa8dbb 100644 --- a/slam/src/MapPoint.cc +++ b/slam/src/MapPoint.cc @@ -350,19 +350,16 @@ namespace ORB_SLAM2 if (mObservations.count(pKF)) { int idx = mObservations[pKF]; + nObs--; if (pKF->mvuRight[idx] >= 0) - nObs -= 2; - else nObs--; mObservations.erase(pKF); - if (mpRefKF == pKF) - mpRefKF = mObservations.begin()->first; + mpRefKF = mpRefKF == pKF ? mObservations.begin()->first : mpRefKF; // If only 2 observations or less, discard point - if (nObs <= 2) - bBad = true; + bBad = nObs <= 2 ? true : bBad; } } diff --git a/slam/src/ORBextractor.cc b/slam/src/ORBextractor.cc index 38e9a53a..cc5d140c 100644 --- a/slam/src/ORBextractor.cc +++ b/slam/src/ORBextractor.cc @@ -79,7 +79,7 @@ uint get_time_diff2(std::chrono::steady_clock::time_point start, std::chrono::st namespace ORB_SLAM2 { - const int PATCH_SIZE = 31; + const int PATCH_SIZE = 5; // PATCH_SIZE was 31 and we changed it to 5 to perform a shift left const int HALF_PATCH_SIZE = 15; const int EDGE_THRESHOLD = 19; @@ -95,14 +95,16 @@ namespace ORB_SLAM2 // Go line by line in the circuI853lar patch int step = (int)image.step1(); + int vMulStep = 0; for (int v = 1; v <= HALF_PATCH_SIZE; ++v) { // Proceed over the two lines int v_sum = 0; int d = u_max[v]; + vMulStep += step; for (int u = -d; u <= d; ++u) { - int val_plus = center[u + v * step], val_minus = center[u - v * step]; + int val_plus = center[u + vMulStep], val_minus = center[u - vMulStep]; v_sum += (val_plus - val_minus); m_10 += u * (val_plus + val_minus); } @@ -161,7 +163,7 @@ namespace ORB_SLAM2 #undef GET_VALUE } - static int bit_pattern_31_[256 * 4] = + static int bit_pattern_31_[256 << 2] = { 8, -3, 9, 5 /*mean (0), correlation (0)*/, 4, 2, 7, -12 /*mean (1.12461e-05), correlation (0.0437584)*/, @@ -552,39 +554,46 @@ namespace ORB_SLAM2 vector ORBextractor::DistributeOctTree(const vector &vToDistributeKeys, const int &minX, const int &maxX, const int &minY, const int &maxY, const int &N, const int &level) { + + float difference1 = maxX - minX; + float difference2 = maxY - minY; // Compute how many initial nodes - const int nIni = round(static_cast(maxX - minX) / (maxY - minY)); + const int nIni = round(difference1 / difference2); - const float hX = static_cast(maxX - minX) / nIni; + const float hX = difference2; list lNodes; vector vpIniNodes; vpIniNodes.resize(nIni); + float hXTotal = 0; + int vToDistributeKeysSize = vToDistributeKeys.size(); for (int i = 0; i < nIni; i++) { ExtractorNode ni; - ni.UL = cv::Point2i(hX * static_cast(i), 0); - ni.UR = cv::Point2i(hX * static_cast(i + 1), 0); - ni.BL = cv::Point2i(ni.UL.x, maxY - minY); - ni.BR = cv::Point2i(ni.UR.x, maxY - minY); - ni.vKeys.reserve(vToDistributeKeys.size()); + ni.UL = cv::Point2i(hXTotal, 0); + hXTotal += hX; + ni.UR = cv::Point2i(hXTotal, 0); + ni.BL = cv::Point2i(ni.UL.x, difference2); + ni.BR = cv::Point2i(ni.UR.x, difference2); + ni.vKeys.reserve(vToDistributeKeysSize); lNodes.push_back(ni); vpIniNodes[i] = &lNodes.back(); } // Associate points to childs - for (size_t i = 0; i < vToDistributeKeys.size(); i++) + for (size_t i = 0; i < vToDistributeKeysSize; i++) { const cv::KeyPoint &kp = vToDistributeKeys[i]; vpIniNodes[kp.pt.x / hX]->vKeys.push_back(kp); } list::iterator lit = lNodes.begin(); + list::iterator litEnd = lNodes.end(); - while (lit != lNodes.end()) + while (lit != litEnd) { if (lit->vKeys.size() == 1) { @@ -602,7 +611,7 @@ namespace ORB_SLAM2 int iteration = 0; vector> vSizeAndPointerToNode; - vSizeAndPointerToNode.reserve(lNodes.size() * 4); + vSizeAndPointerToNode.reserve(lNodes.size() << 2); while (!bFinish) { @@ -631,43 +640,47 @@ namespace ORB_SLAM2 lit->DivideNode(n1, n2, n3, n4); // Add childs if they contain points - if (n1.vKeys.size() > 0) + int n1vKeysSize = n1.vKeys.size(); + if (n1vKeysSize > 0) { lNodes.push_front(n1); - if (n1.vKeys.size() > 1) + if (n1vKeysSize > 1) { nToExpand++; - vSizeAndPointerToNode.push_back(make_pair(n1.vKeys.size(), &lNodes.front())); + vSizeAndPointerToNode.push_back(make_pair(n1vKeysSize, &lNodes.front())); lNodes.front().lit = lNodes.begin(); } } - if (n2.vKeys.size() > 0) + int n2vKeysSize = n2.vKeys.size(); + if (n2vKeysSize > 0) { lNodes.push_front(n2); - if (n2.vKeys.size() > 1) + if (n2vKeysSize > 1) { nToExpand++; - vSizeAndPointerToNode.push_back(make_pair(n2.vKeys.size(), &lNodes.front())); + vSizeAndPointerToNode.push_back(make_pair(n2vKeysSize, &lNodes.front())); lNodes.front().lit = lNodes.begin(); } } - if (n3.vKeys.size() > 0) + int n3vKeysSize = n3.vKeys.size(); + if (n3vKeysSize > 0) { lNodes.push_front(n3); - if (n3.vKeys.size() > 1) + if (n3vKeysSize > 1) { nToExpand++; - vSizeAndPointerToNode.push_back(make_pair(n3.vKeys.size(), &lNodes.front())); + vSizeAndPointerToNode.push_back(make_pair(n3vKeysSize, &lNodes.front())); lNodes.front().lit = lNodes.begin(); } } - if (n4.vKeys.size() > 0) + int n4vKeysSize = n4.vKeys.size(); + if (n4vKeysSize > 0) { lNodes.push_front(n4); - if (n4.vKeys.size() > 1) + if (n4vKeysSize > 1) { nToExpand++; - vSizeAndPointerToNode.push_back(make_pair(n4.vKeys.size(), &lNodes.front())); + vSizeAndPointerToNode.push_back(make_pair(n4vKeysSize, &lNodes.front())); lNodes.front().lit = lNodes.begin(); } } @@ -683,7 +696,7 @@ namespace ORB_SLAM2 { bFinish = true; } - else if (((int)lNodes.size() + nToExpand * 3) > N) + else if (((int)lNodes.size() + ((nToExpand << 1) + nToExpand)) > N) { while (!bFinish) @@ -701,39 +714,43 @@ namespace ORB_SLAM2 vPrevSizeAndPointerToNode[j].second->DivideNode(n1, n2, n3, n4); // Add childs if they contain points - if (n1.vKeys.size() > 0) + int n1vKeysSize = n1.vKeys.size(); + if (n1vKeysSize > 0) { lNodes.push_front(n1); - if (n1.vKeys.size() > 1) + if (n1vKeysSize > 1) { - vSizeAndPointerToNode.push_back(make_pair(n1.vKeys.size(), &lNodes.front())); + vSizeAndPointerToNode.push_back(make_pair(n1vKeysSize, &lNodes.front())); lNodes.front().lit = lNodes.begin(); } } - if (n2.vKeys.size() > 0) + int n2vKeysSize = n2.vKeys.size(); + if (n2vKeysSize > 0) { lNodes.push_front(n2); - if (n2.vKeys.size() > 1) + if (n2vKeysSize > 1) { - vSizeAndPointerToNode.push_back(make_pair(n2.vKeys.size(), &lNodes.front())); + vSizeAndPointerToNode.push_back(make_pair(n2vKeysSize, &lNodes.front())); lNodes.front().lit = lNodes.begin(); } } - if (n3.vKeys.size() > 0) + int n3vKeysSize = n3.vKeys.size(); + if (n3vKeysSize > 0) { lNodes.push_front(n3); - if (n3.vKeys.size() > 1) + if (n3vKeysSize > 1) { - vSizeAndPointerToNode.push_back(make_pair(n3.vKeys.size(), &lNodes.front())); + vSizeAndPointerToNode.push_back(make_pair(n3vKeysSize, &lNodes.front())); lNodes.front().lit = lNodes.begin(); } } - if (n4.vKeys.size() > 0) + int n4vKeysSize = n4.vKeys.size(); + if (n4vKeysSize > 0) { lNodes.push_front(n4); - if (n4.vKeys.size() > 1) + if (n4vKeysSize > 1) { - vSizeAndPointerToNode.push_back(make_pair(n4.vKeys.size(), &lNodes.front())); + vSizeAndPointerToNode.push_back(make_pair(n4vKeysSize, &lNodes.front())); lNodes.front().lit = lNodes.begin(); } } @@ -759,7 +776,8 @@ namespace ORB_SLAM2 cv::KeyPoint *pKP = &vNodeKeys[0]; float maxResponse = pKP->response; - for (size_t k = 1; k < vNodeKeys.size(); k++) + int vNodeKeysSize = vNodeKeys.size(); + for (size_t k = 1; k < vNodeKeysSize; k++) { if (vNodeKeys[k].response > maxResponse) { @@ -778,7 +796,12 @@ namespace ORB_SLAM2 { allKeypoints.resize(nlevels); - const float W = 30; + + // No need to keep W as float, int is better + const int W = 5; // W must be 32, but we prefered to make it 5 since 2^5 = 32 and then use shift + /*Increasing the value of W will result in larger grid cells covering the image. This means fewer keypoints will be extracted, and they will be more spread out across the image. + Larger cells can lead to better coverage of the image, but they might also miss smaller details and features. + Fewer keypoints can lead to faster processing but might sacrifice fine-grained feature detection.*/ for (int level = 0; level < nlevels; ++level) { @@ -788,18 +811,22 @@ namespace ORB_SLAM2 const int maxBorderY = mvImagePyramid[level].rows - EDGE_THRESHOLD + 3; vector vToDistributeKeys; - vToDistributeKeys.reserve(nfeatures * 10); + vToDistributeKeys.reserve((nfeatures << 3) + (nfeatures << 1)); - const float width = (maxBorderX - minBorderX); - const float height = (maxBorderY - minBorderY); + // No need for width and height to be float too + const int width = (maxBorderX - minBorderX); + const int height = (maxBorderY - minBorderY); - const int nCols = width / W; - const int nRows = height / W; - const int wCell = ceil(width / nCols); - const int hCell = ceil(height / nRows); + const int nCols = width >> W; + const int nRows = height >> W; + const int wCell = ceil((float)width / (float)nCols); + const int hCell = ceil((float)height / (float)nRows); + + float hCellTotal = 0; for (int i = 0; i < nRows; i++) { - const float iniY = minBorderY + i * hCell; + const float iniY = minBorderY + hCellTotal; + hCellTotal += hCell; float maxY = iniY + hCell + 6; if (iniY >= maxBorderY - 3) @@ -807,9 +834,11 @@ namespace ORB_SLAM2 if (maxY > maxBorderY) maxY = maxBorderY; + float wCellTotal = 0; for (int j = 0; j < nCols; j++) { - const float iniX = minBorderX + j * wCell; + const float iniX = minBorderX + wCellTotal; + wCellTotal += wCell; float maxX = iniX + wCell + 6; if (iniX >= maxBorderX - 6) continue; @@ -826,12 +855,12 @@ namespace ORB_SLAM2 vKeysCell, minThFAST, true); } - if (!vKeysCell.empty()) + else { for (vector::iterator vit = vKeysCell.begin(); vit != vKeysCell.end(); vit++) { - (*vit).pt.x += j * wCell; - (*vit).pt.y += i * hCell; + (*vit).pt.x += wCellTotal; + (*vit).pt.y += hCellTotal; cv::KeyPoint kp = (*vit); //vToDistributeKeys.push_back(*vit); vToDistributeKeys.emplace_back(kp); @@ -847,7 +876,7 @@ namespace ORB_SLAM2 keypoints = DistributeOctTree(vToDistributeKeys, minBorderX, maxBorderX, minBorderY, maxBorderY, mnFeaturesPerLevel[level], level); - const int scaledPatchSize = PATCH_SIZE * mvScaleFactor[level]; + const int scaledPatchSize = (int)mvScaleFactor[level] << PATCH_SIZE; // Add border to coordinates and scale information const int nkps = keypoints.size(); @@ -1002,7 +1031,7 @@ namespace ORB_SLAM2 vector &keypoints = allKeypoints[level]; keypoints.reserve(nDesiredFeatures * 2); - const int scaledPatchSize = PATCH_SIZE * mvScaleFactor[level]; + const int scaledPatchSize = (int)mvScaleFactor[level] << PATCH_SIZE; // Retain by score and transform coordinates for (int i = 0; i < levelRows; i++) diff --git a/slam/src/ORBmatcher.cc b/slam/src/ORBmatcher.cc index 27f319a4..0eb636ac 100644 --- a/slam/src/ORBmatcher.cc +++ b/slam/src/ORBmatcher.cc @@ -1330,110 +1330,111 @@ namespace ORB_SLAM2 rotHist[i].reserve(500); const float factor = 1.0f / HISTO_LENGTH; - const cv::Mat Rcw = CurrentFrame.mTcw.rowRange(0, 3).colRange(0, 3); - const cv::Mat tcw = CurrentFrame.mTcw.rowRange(0, 3).col(3); + const cv::Mat Rcw = CurrentFrame.mRcw; + const cv::Mat tcw = CurrentFrame.mtcw; - const cv::Mat twc = -Rcw.t() * tcw; + const cv::Mat twc = CurrentFrame.mOw; - const cv::Mat Rlw = LastFrame.mTcw.rowRange(0, 3).colRange(0, 3); - const cv::Mat tlw = LastFrame.mTcw.rowRange(0, 3).col(3); + const cv::Mat Rlw = LastFrame.mRcw; + const cv::Mat tlw = LastFrame.mtcw; const cv::Mat tlc = Rlw * twc + tlw; const bool bForward = tlc.at(2) > CurrentFrame.mb && !bMono; const bool bBackward = -tlc.at(2) > CurrentFrame.mb && !bMono; - for (int i = 0; i < LastFrame.N; i++) + int lastFrameKeyPoints = LastFrame.N; + for (int i = 0; i < lastFrameKeyPoints; i++) { MapPoint *pMP = LastFrame.mvpMapPoints[i]; - if (pMP) + if (pMP && !LastFrame.mvbOutlier[i]) { - if (!LastFrame.mvbOutlier[i]) - { - // Project - cv::Mat x3Dw = pMP->GetWorldPos(); - cv::Mat x3Dc = Rcw * x3Dw + tcw; + // Project + cv::Mat x3Dw = pMP->GetWorldPos(); + cv::Mat x3Dc = Rcw * x3Dw + tcw; - const float xc = x3Dc.at(0); - const float yc = x3Dc.at(1); - const float invzc = 1.0 / x3Dc.at(2); + const float xc = x3Dc.at(0); + const float yc = x3Dc.at(1); + const float invzc = 1.0 / x3Dc.at(2); - if (invzc < 0) - continue; + if (invzc < 0) + continue; - float u = CurrentFrame.fx * xc * invzc + CurrentFrame.cx; - float v = CurrentFrame.fy * yc * invzc + CurrentFrame.cy; + float u = CurrentFrame.fx * xc * invzc + CurrentFrame.cx; + float v = CurrentFrame.fy * yc * invzc + CurrentFrame.cy; - if (u < CurrentFrame.mnMinX || u > CurrentFrame.mnMaxX) - continue; - if (v < CurrentFrame.mnMinY || v > CurrentFrame.mnMaxY) - continue; + if ((u < CurrentFrame.mnMinX || u > CurrentFrame.mnMaxX) || + (v < CurrentFrame.mnMinY || v > CurrentFrame.mnMaxY)) + { + continue; + } - int nLastOctave = LastFrame.mvKeys[i].octave; + int nLastOctave = LastFrame.mvKeys[i].octave; - // Search in a window. Size depends on scale - float radius = th * CurrentFrame.mvScaleFactors[nLastOctave]; + // Search in a window. Size depends on scale + float radius = th * CurrentFrame.mvScaleFactors[nLastOctave]; - vector vIndices2; + vector vIndices2; - if (bForward) - vIndices2 = CurrentFrame.GetFeaturesInArea(u, v, radius, nLastOctave); - else if (bBackward) - vIndices2 = CurrentFrame.GetFeaturesInArea(u, v, radius, 0, nLastOctave); - else - vIndices2 = CurrentFrame.GetFeaturesInArea(u, v, radius, nLastOctave - 1, nLastOctave + 1); + if (bForward) + vIndices2 = CurrentFrame.GetFeaturesInArea(u, v, radius, nLastOctave); + else if (bBackward) + vIndices2 = CurrentFrame.GetFeaturesInArea(u, v, radius, 0, nLastOctave); + else + vIndices2 = CurrentFrame.GetFeaturesInArea(u, v, radius, nLastOctave - 1, nLastOctave + 1); - if (vIndices2.empty()) - continue; + if (vIndices2.empty()) + continue; - const cv::Mat dMP = pMP->GetDescriptor(); + const cv::Mat dMP = pMP->GetDescriptor(); - int bestDist = 256; - int bestIdx2 = -1; + int bestDist = 256; + int bestIdx2 = -1; - for (vector::const_iterator vit = vIndices2.begin(), vend = vIndices2.end(); vit != vend; vit++) + for (vector::const_iterator vit = vIndices2.begin(), vend = vIndices2.end(); vit != vend; vit++) + { + const size_t i2 = *vit; + if (CurrentFrame.mvpMapPoints[i2] && + CurrentFrame.mvpMapPoints[i2]->Observations() > 0) { - const size_t i2 = *vit; - if (CurrentFrame.mvpMapPoints[i2]) - if (CurrentFrame.mvpMapPoints[i2]->Observations() > 0) - continue; + continue; + } - if (CurrentFrame.mvuRight[i2] > 0) - { - const float ur = u - CurrentFrame.mbf * invzc; - const float er = fabs(ur - CurrentFrame.mvuRight[i2]); - if (er > radius) - continue; - } + if (CurrentFrame.mvuRight[i2] > 0) + { + const float ur = u - CurrentFrame.mbf * invzc; + const float er = fabs(ur - CurrentFrame.mvuRight[i2]); + if (er > radius) + continue; + } - const cv::Mat &d = CurrentFrame.mDescriptors.row(i2); + const cv::Mat& d = CurrentFrame.mDescriptors.row(i2); - const int dist = DescriptorDistance(dMP, d); + const int dist = DescriptorDistance(dMP, d); - if (dist < bestDist) - { - bestDist = dist; - bestIdx2 = i2; - } + if (dist < bestDist) + { + bestDist = dist; + bestIdx2 = i2; } + } - if (bestDist <= TH_HIGH) - { - CurrentFrame.mvpMapPoints[bestIdx2] = pMP; - nmatches++; + if (bestDist <= TH_HIGH) + { + CurrentFrame.mvpMapPoints[bestIdx2] = pMP; + nmatches++; - if (mbCheckOrientation) - { - float rot = LastFrame.mvKeysUn[i].angle - CurrentFrame.mvKeysUn[bestIdx2].angle; - if (rot < 0.0) - rot += 360.0f; - int bin = round(rot * factor); - if (bin == HISTO_LENGTH) - bin = 0; - assert(bin >= 0 && bin < HISTO_LENGTH); - rotHist[bin].push_back(bestIdx2); - } + if (mbCheckOrientation) + { + float rot = LastFrame.mvKeysUn[i].angle - CurrentFrame.mvKeysUn[bestIdx2].angle; + if (rot < 0.0) + rot += 360.0f; + int bin = round(rot * factor); + if (bin == HISTO_LENGTH) + bin = 0; + assert(bin >= 0 && bin < HISTO_LENGTH); + rotHist[bin].push_back(bestIdx2); } } } @@ -1452,7 +1453,8 @@ namespace ORB_SLAM2 { if (i != ind1 && i != ind2 && i != ind3) { - for (size_t j = 0, jend = rotHist[i].size(); j < jend; j++) + int rotHistSize = rotHist[i].size(); + for (size_t j = 0, jend = rotHistSize; j < jend; j++) { CurrentFrame.mvpMapPoints[rotHist[i][j]] = static_cast(NULL); nmatches--; @@ -1468,9 +1470,9 @@ namespace ORB_SLAM2 { int nmatches = 0; - const cv::Mat Rcw = CurrentFrame.mTcw.rowRange(0, 3).colRange(0, 3); - const cv::Mat tcw = CurrentFrame.mTcw.rowRange(0, 3).col(3); - const cv::Mat Ow = -Rcw.t() * tcw; + const cv::Mat Rcw = CurrentFrame.mRcw; + const cv::Mat tcw = CurrentFrame.mtcw; + const cv::Mat Ow = CurrentFrame.mOw; // Rotation Histogram (to check rotation consistency) vector rotHist[HISTO_LENGTH]; @@ -1580,7 +1582,8 @@ namespace ORB_SLAM2 { if (i != ind1 && i != ind2 && i != ind3) { - for (size_t j = 0, jend = rotHist[i].size(); j < jend; j++) + int rotHistSize = rotHist[i].size(); + for (size_t j = 0, jend = rotHistSize; j < jend; j++) { CurrentFrame.mvpMapPoints[rotHist[i][j]] = NULL; nmatches--; diff --git a/slam/src/Optimizer.cc b/slam/src/Optimizer.cc index c7a970a5..7932d106 100644 --- a/slam/src/Optimizer.cc +++ b/slam/src/Optimizer.cc @@ -235,22 +235,22 @@ void Optimizer::BundleAdjustment(const vector &vpKFs, const vector(); - g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver); + g2o::BlockSolver_6_3* solver_ptr = new g2o::BlockSolver_6_3(linearSolver); g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); optimizer.setAlgorithm(solver); - int nInitialCorrespondences=0; + int nInitialCorrespondences = 0; // Set Frame vertex - g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap(); + g2o::VertexSE3Expmap* vSE3 = new g2o::VertexSE3Expmap(); vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw)); vSE3->setId(0); vSE3->setFixed(false); @@ -274,21 +274,20 @@ int Optimizer::PoseOptimization(Frame *pFrame) { - unique_lock lock(MapPoint::mGlobalMutex); + unique_lock lock(MapPoint::mGlobalMutex); - for(int i=0; imvpMapPoints[i]; - if(pMP) + for (int i = 0; i < N; i++) { + MapPoint* pMP = pFrame->mvpMapPoints[i]; // Monocular observation - if(pFrame->mvuRight[i]<0) + if (pMP && pFrame->mvuRight[i] < 0) { nInitialCorrespondences++; pFrame->mvbOutlier[i] = false; - Eigen::Matrix obs; - const cv::KeyPoint &kpUn = pFrame->mvKeysUn[i]; + /*Eigen::Matrix obs;*/ + Eigen::Vector2d obs; + const cv::KeyPoint& kpUn = pFrame->mvKeysUn[i]; obs << kpUn.pt.x, kpUn.pt.y; g2o::EdgeSE3ProjectXYZOnlyPose* e = new g2o::EdgeSE3ProjectXYZOnlyPose(); @@ -296,7 +295,7 @@ int Optimizer::PoseOptimization(Frame *pFrame) e->setVertex(0, dynamic_cast(optimizer.vertex(0))); e->setMeasurement(obs); const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave]; - e->setInformation(Eigen::Matrix2d::Identity()*invSigma2); + e->setInformation(Eigen::Matrix2d::Identity() * invSigma2); g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber; e->setRobustKernel(rk); @@ -315,128 +314,60 @@ int Optimizer::PoseOptimization(Frame *pFrame) vpEdgesMono.push_back(e); vnIndexEdgeMono.push_back(i); - } - else // Stereo observation - { - nInitialCorrespondences++; - pFrame->mvbOutlier[i] = false; - - //SET EDGE - Eigen::Matrix obs; - const cv::KeyPoint &kpUn = pFrame->mvKeysUn[i]; - const float &kp_ur = pFrame->mvuRight[i]; - obs << kpUn.pt.x, kpUn.pt.y, kp_ur; - - g2o::EdgeStereoSE3ProjectXYZOnlyPose* e = new g2o::EdgeStereoSE3ProjectXYZOnlyPose(); - e->setVertex(0, dynamic_cast(optimizer.vertex(0))); - e->setMeasurement(obs); - const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave]; - Eigen::Matrix3d Info = Eigen::Matrix3d::Identity()*invSigma2; - e->setInformation(Info); - - g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber; - e->setRobustKernel(rk); - rk->setDelta(deltaStereo); - - e->fx = pFrame->fx; - e->fy = pFrame->fy; - e->cx = pFrame->cx; - e->cy = pFrame->cy; - e->bf = pFrame->mbf; - cv::Mat Xw = pMP->GetWorldPos(); - e->Xw[0] = Xw.at(0); - e->Xw[1] = Xw.at(1); - e->Xw[2] = Xw.at(2); - - optimizer.addEdge(e); - - vpEdgesStereo.push_back(e); - vnIndexEdgeStereo.push_back(i); } } - - } } - if(nInitialCorrespondences<3) + if (nInitialCorrespondences < 3) return 0; // We perform 4 optimizations, after each optimization we classify observation as inlier/outlier // At the next optimization, outliers are not included, but at the end they can be classified as inliers again. - const float chi2Mono[4]={5.991,5.991,5.991,5.991}; - const float chi2Stereo[4]={7.815,7.815,7.815, 7.815}; - const int its[4]={10,10,10,10}; + const float chi2Mono[4] = { 5.991,5.991,5.991,5.991 }; + const float chi2Stereo[4] = { 7.815,7.815,7.815, 7.815 }; + const int its[4] = { 10,10,10,10 }; - int nBad=0; - for(size_t it=0; it<4; it++) + int nBad = 0; + for (size_t it = 0; it < 4; it++) { vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw)); optimizer.initializeOptimization(0); optimizer.optimize(its[it]); - nBad=0; - for(size_t i=0, iend=vpEdgesMono.size(); imvbOutlier[idx]) - { - e->computeError(); - } - - const float chi2 = e->chi2(); - - if(chi2>chi2Mono[it]) - { - pFrame->mvbOutlier[idx]=true; - e->setLevel(1); - nBad++; - } - else - { - pFrame->mvbOutlier[idx]=false; - e->setLevel(0); - } - - if(it==2) - e->setRobustKernel(0); - } - - for(size_t i=0, iend=vpEdgesStereo.size(); imvbOutlier[idx]) + if (pFrame->mvbOutlier[idx]) { e->computeError(); } const float chi2 = e->chi2(); - if(chi2>chi2Stereo[it]) + if (chi2 > chi2Mono[it]) { - pFrame->mvbOutlier[idx]=true; + pFrame->mvbOutlier[idx] = true; e->setLevel(1); nBad++; } else { + pFrame->mvbOutlier[idx] = false; e->setLevel(0); - pFrame->mvbOutlier[idx]=false; } - if(it==2) + if (it == 2) e->setRobustKernel(0); } - - if(optimizer.edges().size()<10) + + if (optimizer.edges().size() < 10) break; } @@ -446,58 +377,71 @@ int Optimizer::PoseOptimization(Frame *pFrame) cv::Mat pose = Converter::toCvMat(SE3quat_recov); pFrame->SetPose(pose); - return nInitialCorrespondences-nBad; + return nInitialCorrespondences - nBad; } -void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap) +void Optimizer::LocalBundleAdjustment(KeyFrame* pKF, bool* pbStopFlag, Map* pMap) { // Local KeyFrames: First Breath Search from Current Keyframe + + // push_back in the list is done dynamically, which means for certain pushes, it will take O(n) + // time to perform. However, after defining a vector with predefined size, it will take O(1) for all cases + list lLocalKeyFrames; lLocalKeyFrames.push_back(pKF); - pKF->mnBALocalForKF = pKF->mnId; const vector vNeighKFs = pKF->GetVectorCovisibleKeyFrames(); - for(int i=0, iend=vNeighKFs.size(); i vLocalKeyFrames; + vLocalKeyFrames.reserve(vNeighKFsSize + 1); + + vLocalKeyFrames.push_back(pKF); + + pKF->mnBALocalForKF = pKF->mnId; + + for (int i = 0, iend = vNeighKFsSize; i < iend; i++) { KeyFrame* pKFi = vNeighKFs[i]; pKFi->mnBALocalForKF = pKF->mnId; - if(!pKFi->isBad()) - lLocalKeyFrames.push_back(pKFi); + if (!pKFi->isBad()) + vLocalKeyFrames.push_back(pKFi); } + lLocalKeyFrames.insert(lLocalKeyFrames.end(), vLocalKeyFrames.begin(), vLocalKeyFrames.end()); + // Local MapPoints seen in Local KeyFrames list lLocalMapPoints; - for(list::iterator lit=lLocalKeyFrames.begin() , lend=lLocalKeyFrames.end(); lit!=lend; lit++) + for (list::iterator lit = lLocalKeyFrames.begin(), lend = lLocalKeyFrames.end(); lit != lend; lit++) { vector vpMPs = (*lit)->GetMapPointMatches(); - for(vector::iterator vit=vpMPs.begin(), vend=vpMPs.end(); vit!=vend; vit++) + for (vector::iterator vit = vpMPs.begin(), vend = vpMPs.end(); vit != vend; vit++) { MapPoint* pMP = *vit; - if(pMP) - if(!pMP->isBad()) - if(pMP->mnBALocalForKF!=pKF->mnId) - { - lLocalMapPoints.push_back(pMP); - pMP->mnBALocalForKF=pKF->mnId; - } + if (pMP && !pMP->isBad() && pMP->mnBALocalForKF != pKF->mnId) + { + lLocalMapPoints.push_back(pMP); + pMP->mnBALocalForKF = pKF->mnId; + } } } // Fixed Keyframes. Keyframes that see Local MapPoints but that are not Local Keyframes list lFixedCameras; - for(list::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++) + for (list::iterator lit = lLocalMapPoints.begin(), lend = lLocalMapPoints.end(); lit != lend; lit++) { - map observations = (*lit)->GetObservations(); - for(map::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++) + map observations = (*lit)->GetObservations(); + for (map::iterator mit = observations.begin(), mend = observations.end(); mit != mend; mit++) { KeyFrame* pKFi = mit->first; - if(pKFi->mnBALocalForKF!=pKF->mnId && pKFi->mnBAFixedForKF!=pKF->mnId) + if (pKFi->mnBALocalForKF != pKF->mnId && pKFi->mnBAFixedForKF != pKF->mnId) { - pKFi->mnBAFixedForKF=pKF->mnId; - if(!pKFi->isBad()) + pKFi->mnBAFixedForKF = pKF->mnId; + if (!pKFi->isBad()) lFixedCameras.push_back(pKFi); } } @@ -505,48 +449,48 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap // Setup optimizer g2o::SparseOptimizer optimizer; - g2o::BlockSolver_6_3::LinearSolverType * linearSolver; + g2o::BlockSolver_6_3::LinearSolverType* linearSolver; linearSolver = new g2o::LinearSolverEigen(); - g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver); + g2o::BlockSolver_6_3* solver_ptr = new g2o::BlockSolver_6_3(linearSolver); g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); optimizer.setAlgorithm(solver); - if(pbStopFlag) + if (pbStopFlag) optimizer.setForceStopFlag(pbStopFlag); unsigned long maxKFid = 0; // Set Local KeyFrame vertices - for(list::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++) + for (list::iterator lit = lLocalKeyFrames.begin(), lend = lLocalKeyFrames.end(); lit != lend; lit++) { KeyFrame* pKFi = *lit; - g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap(); + g2o::VertexSE3Expmap* vSE3 = new g2o::VertexSE3Expmap(); vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose())); vSE3->setId(pKFi->mnId); - vSE3->setFixed(pKFi->mnId==0); + vSE3->setFixed(pKFi->mnId == 0); optimizer.addVertex(vSE3); - if(pKFi->mnId>maxKFid) - maxKFid=pKFi->mnId; + if (pKFi->mnId > maxKFid) + maxKFid = pKFi->mnId; } // Set Fixed KeyFrame vertices - for(list::iterator lit=lFixedCameras.begin(), lend=lFixedCameras.end(); lit!=lend; lit++) + for (list::iterator lit = lFixedCameras.begin(), lend = lFixedCameras.end(); lit != lend; lit++) { KeyFrame* pKFi = *lit; - g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap(); + g2o::VertexSE3Expmap* vSE3 = new g2o::VertexSE3Expmap(); vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose())); vSE3->setId(pKFi->mnId); vSE3->setFixed(true); optimizer.addVertex(vSE3); - if(pKFi->mnId>maxKFid) - maxKFid=pKFi->mnId; + if (pKFi->mnId > maxKFid) + maxKFid = pKFi->mnId; } // Set MapPoint vertices - const int nExpectedSize = (lLocalKeyFrames.size()+lFixedCameras.size())*lLocalMapPoints.size(); + const int nExpectedSize = (lLocalKeyFrames.size() + lFixedCameras.size()) * lLocalMapPoints.size(); vector vpEdgesMono; vpEdgesMono.reserve(nExpectedSize); @@ -569,40 +513,39 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap const float thHuberMono = sqrt(5.991); const float thHuberStereo = sqrt(7.815); - for(list::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++) + for (list::iterator lit = lLocalMapPoints.begin(), lend = lLocalMapPoints.end(); lit != lend; lit++) { MapPoint* pMP = *lit; g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ(); vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos())); - int id = pMP->mnId+maxKFid+1; + int id = pMP->mnId + maxKFid + 1; vPoint->setId(id); vPoint->setMarginalized(true); optimizer.addVertex(vPoint); - const map observations = pMP->GetObservations(); + const map observations = pMP->GetObservations(); //Set edges - for(map::const_iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++) + for (map::const_iterator mit = observations.begin(), mend = observations.end(); mit != mend; mit++) { KeyFrame* pKFi = mit->first; - if(!pKFi->isBad()) + if (!pKFi->isBad()) { - const cv::KeyPoint &kpUn = pKFi->mvKeysUn[mit->second]; + const cv::KeyPoint& kpUn = pKFi->mvKeysUn[mit->second]; // Monocular observation - if(pKFi->mvuRight[mit->second]<0) + if (pKFi->mvuRight[mit->second] < 0) { - Eigen::Matrix obs; - obs << kpUn.pt.x, kpUn.pt.y; + Eigen::Vector2d obs(kpUn.pt.x, kpUn.pt.y); g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ(); e->setVertex(0, dynamic_cast(optimizer.vertex(id))); e->setVertex(1, dynamic_cast(optimizer.vertex(pKFi->mnId))); e->setMeasurement(obs); - const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave]; - e->setInformation(Eigen::Matrix2d::Identity()*invSigma2); + const float& invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave]; + e->setInformation(Eigen::Matrix2d::Identity() * invSigma2); g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber; e->setRobustKernel(rk); @@ -618,139 +561,87 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap vpEdgeKFMono.push_back(pKFi); vpMapPointEdgeMono.push_back(pMP); } - else // Stereo observation - { - std::cout << "STEREO !!!!" << std::endl; - Eigen::Matrix obs; - const float kp_ur = pKFi->mvuRight[mit->second]; - obs << kpUn.pt.x, kpUn.pt.y, kp_ur; - - g2o::EdgeStereoSE3ProjectXYZ* e = new g2o::EdgeStereoSE3ProjectXYZ(); - - e->setVertex(0, dynamic_cast(optimizer.vertex(id))); - e->setVertex(1, dynamic_cast(optimizer.vertex(pKFi->mnId))); - e->setMeasurement(obs); - const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave]; - Eigen::Matrix3d Info = Eigen::Matrix3d::Identity()*invSigma2; - e->setInformation(Info); - - g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber; - e->setRobustKernel(rk); - rk->setDelta(thHuberStereo); - - e->fx = pKFi->fx; - e->fy = pKFi->fy; - e->cx = pKFi->cx; - e->cy = pKFi->cy; - e->bf = pKFi->mbf; - - optimizer.addEdge(e); - vpEdgesStereo.push_back(e); - vpEdgeKFStereo.push_back(pKFi); - vpMapPointEdgeStereo.push_back(pMP); - } } } } - if(pbStopFlag) - if(*pbStopFlag) - return; + if (pbStopFlag && *pbStopFlag) + return; optimizer.initializeOptimization(); optimizer.optimize(5); - - - bool bDoMore= true; - - if(pbStopFlag) - if(*pbStopFlag) - bDoMore = false; - - if(bDoMore) + if (!(pbStopFlag && *pbStopFlag)) { - // Check inlier observations - for(size_t i=0, iend=vpEdgesMono.size(); iisBad()) - continue; + if (pMP->isBad()) + continue; - if(e->chi2()>5.991 || !e->isDepthPositive()) - { - e->setLevel(1); + if (e->chi2() > 5.991 || !e->isDepthPositive()) + { + e->setLevel(1); + } + + e->setRobustKernel(0); } - e->setRobustKernel(0); - } + for (size_t i = 0, iend = vpEdgesStereo.size(); i < iend; i++) + { + g2o::EdgeStereoSE3ProjectXYZ* e = vpEdgesStereo[i]; + MapPoint* pMP = vpMapPointEdgeStereo[i]; - for(size_t i=0, iend=vpEdgesStereo.size(); iisBad()) + continue; - if(pMP->isBad()) - continue; + if (e->chi2() > 7.815 || !e->isDepthPositive()) + { + e->setLevel(1); + } - if(e->chi2()>7.815 || !e->isDepthPositive()) - { - e->setLevel(1); + e->setRobustKernel(0); } - e->setRobustKernel(0); - } - - // Optimize again without the outliers + // Optimize again without the outliers - optimizer.initializeOptimization(0); - optimizer.optimize(10); + optimizer.initializeOptimization(0); + optimizer.optimize(10); } - vector > vToErase; - vToErase.reserve(vpEdgesMono.size()+vpEdgesStereo.size()); + vector > vToErase; + vToErase.reserve(vpEdgesMono.size() + vpEdgesStereo.size()); // Check inlier observations - for(size_t i=0, iend=vpEdgesMono.size(); iisBad()) - continue; - - if(e->chi2()>5.991 || !e->isDepthPositive()) + for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++) { - KeyFrame* pKFi = vpEdgeKFMono[i]; - vToErase.push_back(make_pair(pKFi,pMP)); - } - } + g2o::EdgeSE3ProjectXYZ* e = vpEdgesMono[i]; + MapPoint* pMP = vpMapPointEdgeMono[i]; - for(size_t i=0, iend=vpEdgesStereo.size(); iisBad()) - continue; + if (pMP->isBad()) + continue; - if(e->chi2()>7.815 || !e->isDepthPositive()) - { - KeyFrame* pKFi = vpEdgeKFStereo[i]; - vToErase.push_back(make_pair(pKFi,pMP)); + if (e->chi2() > 5.991 || !e->isDepthPositive()) + { + KeyFrame* pKFi = vpEdgeKFMono[i]; + vToErase.push_back(make_pair(pKFi, pMP)); + } } } // Get Map Mutex unique_lock lock(pMap->mMutexMapUpdate); - if(!vToErase.empty()) + if (!vToErase.empty()) { - for(size_t i=0;i::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++) + for (list::iterator lit = lLocalKeyFrames.begin(), lend = lLocalKeyFrames.end(); lit != lend; lit++) { KeyFrame* pKF = *lit; g2o::VertexSE3Expmap* vSE3 = static_cast(optimizer.vertex(pKF->mnId)); @@ -771,10 +662,10 @@ void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap } //Points - for(list::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++) + for (list::iterator lit = lLocalMapPoints.begin(), lend = lLocalMapPoints.end(); lit != lend; lit++) { MapPoint* pMP = *lit; - g2o::VertexSBAPointXYZ* vPoint = static_cast(optimizer.vertex(pMP->mnId+maxKFid+1)); + g2o::VertexSBAPointXYZ* vPoint = static_cast(optimizer.vertex(pMP->mnId + maxKFid + 1)); pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate())); pMP->UpdateNormalAndDepth(); } diff --git a/slam/src/Tracking.cc b/slam/src/Tracking.cc index bd2d931f..17e38c82 100644 --- a/slam/src/Tracking.cc +++ b/slam/src/Tracking.cc @@ -862,19 +862,17 @@ namespace ORB_SLAM2 void Tracking::UpdateLastFrame() { // Update pose according to reference keyframe - KeyFrame *pRef = mLastFrame.mpReferenceKF; - cv::Mat Tlr = mlRelativeFramePoses.back(); + mLastFrame.SetPose(mlRelativeFramePoses.back() * mLastFrame.mpReferenceKF->GetPose()); - mLastFrame.SetPose(Tlr * pRef->GetPose()); - - if (mnLastKeyFrameId == mLastFrame.mnId || mSensor == System::MONOCULAR || !mbOnlyTracking) + if (mnLastKeyFrameId == mLastFrame.mnId || mSensor || !mbOnlyTracking) return; // Create "visual odometry" MapPoints // We sort points according to their measured depth by the stereo/RGB-D sensor vector> vDepthIdx; - vDepthIdx.reserve(mLastFrame.N); - for (int i = 0; i < mLastFrame.N; i++) + int mLastFrameKeyPoints = mLastFrame.N; + vDepthIdx.reserve(mLastFrameKeyPoints); + for (int i = 0; i < mLastFrameKeyPoints; i++) { float z = mLastFrame.mvDepth[i]; if (z > 0) @@ -891,35 +889,24 @@ namespace ORB_SLAM2 // We insert all close points (depthObservations() < 1) - { - bCreateNew = true; - } - - if (bCreateNew) + if (!pMP || pMP->Observations() < 1) { cv::Mat x3D = mLastFrame.UnprojectStereo(i); - MapPoint *pNewMP = new MapPoint(x3D, mpMap, &mLastFrame, i); + MapPoint* pNewMP = new MapPoint(x3D, mpMap, &mLastFrame, i); mLastFrame.mvpMapPoints[i] = pNewMP; mlpTemporalPoints.push_back(pNewMP); - nPoints++; - } - else - { - nPoints++; } + nPoints++; + if (vDepthIdx[j].first > mThDepth && nPoints > 100) break; } @@ -935,21 +922,17 @@ namespace ORB_SLAM2 mCurrentFrame.SetPose(mVelocity * mLastFrame.mTcw); - fill(mCurrentFrame.mvpMapPoints.begin(), mCurrentFrame.mvpMapPoints.end(), static_cast(NULL)); + fill(mCurrentFrame.mvpMapPoints.begin(), mCurrentFrame.mvpMapPoints.end(), static_cast(NULL)); // Project points seen in previous frame - int th; - if (mSensor != System::STEREO) - th = 15; - else - th = 7; - int nmatches = matcher.SearchByProjection(mCurrentFrame, mLastFrame, th, mSensor == System::MONOCULAR); + int nmatches = matcher.SearchByProjection(mCurrentFrame, mLastFrame, 15, true); // If few matches, uses a wider window search if (nmatches < 20) { - fill(mCurrentFrame.mvpMapPoints.begin(), mCurrentFrame.mvpMapPoints.end(), static_cast(NULL)); - nmatches = matcher.SearchByProjection(mCurrentFrame, mLastFrame, 2 * th, mSensor == System::MONOCULAR); + fill(mCurrentFrame.mvpMapPoints.begin(), mCurrentFrame.mvpMapPoints.end(), static_cast(NULL)); + + nmatches = matcher.SearchByProjection(mCurrentFrame, mLastFrame, 30, true); } if (nmatches < 20) @@ -960,21 +943,22 @@ namespace ORB_SLAM2 // Discard outliers int nmatchesMap = 0; - for (int i = 0; i < mCurrentFrame.N; i++) + + int numberOfKeyPoints = mCurrentFrame.N; + for (int i = 0; i < numberOfKeyPoints; i++) { - if (mCurrentFrame.mvpMapPoints[i]) + MapPoint* pMP = mCurrentFrame.mvpMapPoints[i]; + if (pMP) { if (mCurrentFrame.mvbOutlier[i]) { - MapPoint *pMP = mCurrentFrame.mvpMapPoints[i]; - - mCurrentFrame.mvpMapPoints[i] = static_cast(NULL); + mCurrentFrame.mvpMapPoints[i] = static_cast(NULL); mCurrentFrame.mvbOutlier[i] = false; pMP->mbTrackInView = false; pMP->mnLastFrameSeen = mCurrentFrame.mnId; nmatches--; } - else if (mCurrentFrame.mvpMapPoints[i]->Observations() > 0) + else if (pMP->Observations() > 0) nmatchesMap++; } } @@ -1035,11 +1019,7 @@ namespace ORB_SLAM2 bool Tracking::NeedNewKeyFrame() { - if (mbOnlyTracking) - return false; - - // If Local Mapping is freezed by a Loop Closure do not insert keyframes - if (mpLocalMapper->isStopped() || mpLocalMapper->stopRequested()) + if (mbOnlyTracking || (mpLocalMapper->isStopped() || mpLocalMapper->stopRequested())) return false; const int nKFs = mpMap->KeyFramesInMap(); @@ -1062,7 +1042,8 @@ namespace ORB_SLAM2 int nTrackedClose = 0; if (mSensor != System::MONOCULAR) { - for (int i = 0; i < mCurrentFrame.N; i++) + int mCurrentFrameN = mCurrentFrame.N; + for (int i = 0; i < mCurrentFrameN; i++) { if (mCurrentFrame.mvDepth[i] > 0 && mCurrentFrame.mvDepth[i] < mThDepth) { @@ -1077,19 +1058,19 @@ namespace ORB_SLAM2 bool bNeedToInsertClose = (nTrackedClose < 100) && (nNonTrackedClose > 70); // Thresholds - float thRefRatio = 0.75f; + float thRefRatio = 0.85f; if (nKFs < 2) - thRefRatio = 0.4f; + thRefRatio = 0.5f; if (mSensor == System::MONOCULAR) - thRefRatio = 0.9f; + thRefRatio = 0.95f; // Condition 1a: More than "MaxFrames" have passed from last keyframe insertion const bool c1a = mCurrentFrame.mnId >= mnLastKeyFrameId + mMaxFrames; // Condition 1b: More than "MinFrames" have passed and Local Mapping is idle const bool c1b = (mCurrentFrame.mnId >= mnLastKeyFrameId + mMinFrames && bLocalMappingIdle); // Condition 1c: tracking is weak - const bool c1c = mSensor != System::MONOCULAR && (mnMatchesInliers < nRefMatches * 0.25 || bNeedToInsertClose); + const bool c1c = mSensor != System::MONOCULAR && (mnMatchesInliers < nRefMatches >> 2 || bNeedToInsertClose); // Condition 2: Few tracked points compared to reference keyframe. Lots of visual odometry compared to map matches. const bool c2 = ((mnMatchesInliers < nRefMatches * thRefRatio || bNeedToInsertClose) && mnMatchesInliers > 15); @@ -1104,15 +1085,7 @@ namespace ORB_SLAM2 else { mpLocalMapper->InterruptBA(); - if (mSensor != System::MONOCULAR) - { - if (mpLocalMapper->KeyframesInQueue() < 3) - return true; - else - return false; - } - else - return false; + return false; } } else