SLAM代碼（一）：直方圖投票

參考鏈接

暫時無

1 ORB-SLAM2 代碼塊

1.1 直方圖投票機制

const int ORBmatcher::HISTO_LENGTH = 30;
-----------------------------------------------------------------
 // Rotation Histogram (to check rotation consistency)      旋轉直方圖(檢查旋轉一致性)
 vector<int> rotHist[HISTO_LENGTH];
 for(int i=0;i<HISTO_LENGTH;i++)
     rotHist[i].reserve(500);
const float factor = HISTO_LENGTH/360.0f;
----------------------------------------------------------------
  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);
  }
----------------------------------------------------
  //Apply rotation consistency
  // 旋轉一致檢測
  if(mbCheckOrientation)
  {
	        int ind1=-1;
	        int ind2=-1;
	        int ind3=-1;

           ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);

		   for(int i=0; i<HISTO_LENGTH; i++)
		   {
		       /**wyp
		        *  如果不屬於三個裏面的統計直方圖裏面最大的三個，那麼就把這個點進行刪除
		        * 
		       */
		       if(i!=ind1 && i!=ind2 && i!=ind3)  
		       {
		           for(size_t j=0, jend=rotHist[i].size(); j<jend; j++)
		           {
		               CurrentFrame.mvpMapPoints[rotHist[i][j]]=static_cast<MapPoint*>(NULL);
		               nmatches--;
		           }
		       }
		   }
}