C++ MapPoint::GetWorldPos方法代码示例

 float KeyFrame::ComputeSceneMedianDepth(int q)
 {
     vector<MapPoint*> vpMapPoints;
     cv::Mat Tcw_;
     {
         boost::mutex::scoped_lock lock(mMutexFeatures);
         boost::mutex::scoped_lock lock2(mMutexPose);
         vpMapPoints = mvpMapPoints;
         Tcw_ = Tcw.clone();
     }
     
     vector<float> vDepths;
     vDepths.reserve(mvpMapPoints.size());
     cv::Mat Rcw2 = Tcw_.row(2).colRange(0,3);
     Rcw2 = Rcw2.t();
     float zcw = Tcw_.at<float>(2,3);
     for(size_t i=0; i<mvpMapPoints.size(); i++)
     {
         if(mvpMapPoints[i])
         {
             MapPoint* pMP = mvpMapPoints[i];
             cv::Mat x3Dw = pMP->GetWorldPos();
             float z = Rcw2.dot(x3Dw)+zcw;
             vDepths.push_back(z);
         }
     }
     
     sort(vDepths.begin(),vDepths.end());
     
     return vDepths[(vDepths.size()-1)/q];
 }

 float KeyFrame::ComputeSceneMedianDepthWithCoord(int &x, int &y, int q) {
     
     //vector<MapPoint*> vpMapPoints;
     cv::Mat Tcw_;
     {
         boost::mutex::scoped_lock lock(mMutexFeatures);
         boost::mutex::scoped_lock lock2(mMutexPose);
         //vpMapPoints = mvpMapPoints;
         Tcw_ = Tcw.clone();
     }
     
     vector<float> vDepths;
     vDepths.reserve(mvpMapPoints.size());
     cv::Mat Rcw2 = Tcw_.row(2).colRange(0,3);
     Rcw2 = Rcw2.t();
     float zcw = Tcw_.at<float>(2,3);
     for(size_t i=0; i<mvpMapPoints.size(); i++)
     {
         if(mvpMapPoints[i])
         {
             MapPoint* pMP = mvpMapPoints[i];
             cv::Mat x3Dw = pMP->GetWorldPos();
             float z = Rcw2.dot(x3Dw)+zcw;
             vDepths.push_back(z);
         }
     }
     
     vector<size_t> depthIndex(vDepths.size());
     for (size_t i = 0; i != depthIndex.size(); ++i) depthIndex[i] = i;
     
     sort(vDepths.begin(),vDepths.end());
     sort(depthIndex.begin(), depthIndex.end(),
          [&vDepths](size_t i1, size_t i2) {return vDepths[i1] < vDepths[i2];});
     
     size_t medianIndex = depthIndex[(vDepths.size()-1)/q];
     size_t ptCounter = 0;
     MapPoint* pointWithMedianDepth;
     for (size_t i = 0; i<mvpMapPoints.size(); i++) {
         if (mvpMapPoints[i]) {
             if (ptCounter == medianIndex) {
                 pointWithMedianDepth = mvpMapPoints[i];
                 break;
             }
             ptCounter++;
         }
     }
     if (pointWithMedianDepth) {
         size_t keysUnIdx = pointWithMedianDepth->GetIndexInKeyFrame(this);
         x = mvKeysUn[keysUnIdx].pt.x;
         y = mvKeysUn[keysUnIdx].pt.y;
     }
     
     return vDepths[(vDepths.size()-1)/q];
 }

 void LoopClosing::CorrectLoop()
 {
     // Send a stop signal to Local Mapping
     // Avoid new keyframes are inserted while correcting the loop
     mpLocalMapper->RequestStop();
     
     // Wait until Local Mapping has effectively stopped
     //ros::Rate r(1e4);
     //while(ros::ok() && !mpLocalMapper->isStopped())
     while(!mpLocalMapper->isStopped())
     {
         //r.sleep();
         boost::this_thread::sleep(boost::posix_time::milliseconds(10000));
     }
     
     // Ensure current keyframe is updated
     mpCurrentKF->UpdateConnections();
     
     // Retrive keyframes connected to the current keyframe and compute corrected Sim3 pose by propagation
     mvpCurrentConnectedKFs = mpCurrentKF->GetVectorCovisibleKeyFrames();
     mvpCurrentConnectedKFs.push_back(mpCurrentKF);
     
     KeyFrameAndPose CorrectedSim3, NonCorrectedSim3;
     CorrectedSim3[mpCurrentKF]=mg2oScw;
     cv::Mat Twc = mpCurrentKF->GetPoseInverse();
     
     
     for(vector<KeyFrame*>::iterator vit=mvpCurrentConnectedKFs.begin(), vend=mvpCurrentConnectedKFs.end(); vit!=vend; vit++)
     {
         KeyFrame* pKFi = *vit;
         
         cv::Mat Tiw = pKFi->GetPose();
         
         if(pKFi!=mpCurrentKF)
         {
             cv::Mat Tic = Tiw*Twc;
             cv::Mat Ric = Tic.rowRange(0,3).colRange(0,3);
             cv::Mat tic = Tic.rowRange(0,3).col(3);
             g2o::Sim3 g2oSic(Converter::toMatrix3d(Ric),Converter::toVector3d(tic),1.0);
             g2o::Sim3 g2oCorrectedSiw = g2oSic*mg2oScw;
             //Pose corrected with the Sim3 of the loop closure
             CorrectedSim3[pKFi]=g2oCorrectedSiw;
         }
         
         cv::Mat Riw = Tiw.rowRange(0,3).colRange(0,3);
         cv::Mat tiw = Tiw.rowRange(0,3).col(3);
         g2o::Sim3 g2oSiw(Converter::toMatrix3d(Riw),Converter::toVector3d(tiw),1.0);
         //Pose without correction
         NonCorrectedSim3[pKFi]=g2oSiw;
     }
     
     // Correct all MapPoints obsrved by current keyframe and neighbors, so that they align with the other side of the loop
     for(KeyFrameAndPose::iterator mit=CorrectedSim3.begin(), mend=CorrectedSim3.end(); mit!=mend; mit++)
     {
         KeyFrame* pKFi = mit->first;
         g2o::Sim3 g2oCorrectedSiw = mit->second;
         g2o::Sim3 g2oCorrectedSwi = g2oCorrectedSiw.inverse();
         
         g2o::Sim3 g2oSiw =NonCorrectedSim3[pKFi];
         
         vector<MapPoint*> vpMPsi = pKFi->GetMapPointMatches();
         for(size_t iMP=0, endMPi = vpMPsi.size(); iMP<endMPi; iMP++)
         {
             MapPoint* pMPi = vpMPsi[iMP];
             if(!pMPi)
                 continue;
             if(pMPi->isBad())
                 continue;
             if(pMPi->mnCorrectedByKF==mpCurrentKF->mnId)
                 continue;
             
             // Project with non-corrected pose and project back with corrected pose
             cv::Mat P3Dw = pMPi->GetWorldPos();
             Eigen::Matrix<double,3,1> eigP3Dw = Converter::toVector3d(P3Dw);
             Eigen::Matrix<double,3,1> eigCorrectedP3Dw = g2oCorrectedSwi.map(g2oSiw.map(eigP3Dw));
             
             cv::Mat cvCorrectedP3Dw = Converter::toCvMat(eigCorrectedP3Dw);
             pMPi->SetWorldPos(cvCorrectedP3Dw);
             pMPi->mnCorrectedByKF = mpCurrentKF->mnId;
             pMPi->mnCorrectedReference = pKFi->mnId;
             pMPi->UpdateNormalAndDepth();
         }
         
         // Update keyframe pose with corrected Sim3. First transform Sim3 to SE3 (scale translation)
         Eigen::Matrix3d eigR = g2oCorrectedSiw.rotation().toRotationMatrix();
         Eigen::Vector3d eigt = g2oCorrectedSiw.translation();
         double s = g2oCorrectedSiw.scale();
         
         eigt *=(1./s); //[R t/s;0 1]
         
         cv::Mat correctedTiw = Converter::toCvSE3(eigR,eigt);
         
         pKFi->SetPose(correctedTiw);
         
         // Make sure connections are updated
         pKFi->UpdateConnections();
     }
     
     // Start Loop Fusion
     // Update matched map points and replace if duplicated
//.........这里部分代码省略.........