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

 void LocalMapping::ProcessNewKeyFrame()
 {
     {
         boost::mutex::scoped_lock lock(mMutexNewKFs);
         mpCurrentKeyFrame = mlNewKeyFrames.front();
         mlNewKeyFrames.pop_front();
     }
     
     // Compute Bags of Words structures
     mpCurrentKeyFrame->ComputeBoW();
     
     if(mpCurrentKeyFrame->mnId==0)
         return;
     
     // Associate MapPoints to the new keyframe and update normal and descriptor
     vector<MapPoint*> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
     if(mpCurrentKeyFrame->mnId>1) //This operations are already done in the tracking for the first two keyframes
     {
         for(size_t i=0; i<vpMapPointMatches.size(); i++)
         {
             MapPoint* pMP = vpMapPointMatches[i];
             if(pMP)
             {
                 if(!pMP->isBad())
                 {
                     pMP->AddObservation(mpCurrentKeyFrame, i);
                     pMP->UpdateNormalAndDepth();
                     pMP->ComputeDistinctiveDescriptors();
                 }
             }
         }
     }
     
     if(mpCurrentKeyFrame->mnId==1)
     {
         for(size_t i=0; i<vpMapPointMatches.size(); i++)
         {
             MapPoint* pMP = vpMapPointMatches[i];
             if(pMP)
             {
                 mlpRecentAddedMapPoints.push_back(pMP);
             }
         }
     }
     
     // Update links in the Covisibility Graph
     mpCurrentKeyFrame->UpdateConnections();
     
     // Insert Keyframe in Map
     mpMap->AddKeyFrame(mpCurrentKeyFrame);
 }

 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
//.........这里部分代码省略.........

//.........这里部分代码省略.........
             const int idx2 = vMatchedIndices[ikp].second;
             
             const cv::KeyPoint &kp1 = vMatchedKeysUn1[ikp];
             const cv::KeyPoint &kp2 = vMatchedKeysUn2[ikp];
             
             // Check parallax between rays
             cv::Mat xn1 = (cv::Mat_<float>(3,1) << (kp1.pt.x-cx1)*invfx1, (kp1.pt.y-cy1)*invfy1, 1.0 );
             cv::Mat ray1 = Rwc1*xn1;
             cv::Mat xn2 = (cv::Mat_<float>(3,1) << (kp2.pt.x-cx2)*invfx2, (kp2.pt.y-cy2)*invfy2, 1.0 );
             cv::Mat ray2 = Rwc2*xn2;
             const float cosParallaxRays = ray1.dot(ray2)/(cv::norm(ray1)*cv::norm(ray2));
             
             if(cosParallaxRays<0 || cosParallaxRays>0.9998)
                 continue;
             
             // Linear Triangulation Method
             cv::Mat A(4,4,CV_32F);
             A.row(0) = xn1.at<float>(0)*Tcw1.row(2)-Tcw1.row(0);
             A.row(1) = xn1.at<float>(1)*Tcw1.row(2)-Tcw1.row(1);
             A.row(2) = xn2.at<float>(0)*Tcw2.row(2)-Tcw2.row(0);
             A.row(3) = xn2.at<float>(1)*Tcw2.row(2)-Tcw2.row(1);
             
             cv::Mat w,u,vt;
             cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
             
             cv::Mat x3D = vt.row(3).t();
             
             if(x3D.at<float>(3)==0)
                 continue;
             
             // Euclidean coordinates
             x3D = x3D.rowRange(0,3)/x3D.at<float>(3);
             cv::Mat x3Dt = x3D.t();
             
             //Check triangulation in front of cameras
             float z1 = Rcw1.row(2).dot(x3Dt)+tcw1.at<float>(2);
             if(z1<=0)
                 continue;
             
             float z2 = Rcw2.row(2).dot(x3Dt)+tcw2.at<float>(2);
             if(z2<=0)
                 continue;
             
             //Check reprojection error in first keyframe
             float sigmaSquare1 = mpCurrentKeyFrame->GetSigma2(kp1.octave);
             float x1 = Rcw1.row(0).dot(x3Dt)+tcw1.at<float>(0);
             float y1 = Rcw1.row(1).dot(x3Dt)+tcw1.at<float>(1);
             float invz1 = 1.0/z1;
             float u1 = fx1*x1*invz1+cx1;
             float v1 = fy1*y1*invz1+cy1;
             float errX1 = u1 - kp1.pt.x;
             float errY1 = v1 - kp1.pt.y;
             if((errX1*errX1+errY1*errY1)>5.991*sigmaSquare1)
                 continue;
             
             //Check reprojection error in second keyframe
             float sigmaSquare2 = pKF2->GetSigma2(kp2.octave);
             float x2 = Rcw2.row(0).dot(x3Dt)+tcw2.at<float>(0);
             float y2 = Rcw2.row(1).dot(x3Dt)+tcw2.at<float>(1);
             float invz2 = 1.0/z2;
             float u2 = fx2*x2*invz2+cx2;
             float v2 = fy2*y2*invz2+cy2;
             float errX2 = u2 - kp2.pt.x;
             float errY2 = v2 - kp2.pt.y;
             if((errX2*errX2+errY2*errY2)>5.991*sigmaSquare2)
                 continue;
             
             //Check scale consistency
             cv::Mat normal1 = x3D-Ow1;
             float dist1 = cv::norm(normal1);
             
             cv::Mat normal2 = x3D-Ow2;
             float dist2 = cv::norm(normal2);
             
             if(dist1==0 || dist2==0)
                 continue;
             
             float ratioDist = dist1/dist2;
             float ratioOctave = mpCurrentKeyFrame->GetScaleFactor(kp1.octave)/pKF2->GetScaleFactor(kp2.octave);
             if(ratioDist*ratioFactor<ratioOctave || ratioDist>ratioOctave*ratioFactor)
                 continue;
             
             // Triangulation is succesfull
             MapPoint* pMP = new MapPoint(x3D,mpCurrentKeyFrame,mpMap);
             
             pMP->AddObservation(pKF2,idx2);
             pMP->AddObservation(mpCurrentKeyFrame,idx1);
             
             mpCurrentKeyFrame->AddMapPoint(pMP,idx1);
             pKF2->AddMapPoint(pMP,idx2);
             
             pMP->ComputeDistinctiveDescriptors();
             
             pMP->UpdateNormalAndDepth();
             
             mpMap->AddMapPoint(pMP);
             mlpRecentAddedMapPoints.push_back(pMP);
         }
     }
 }