diff --git a/src/LocalMapping.cc b/src/LocalMapping.cc index 8f4f94d..895c576 100644 --- a/src/LocalMapping.cc +++ b/src/LocalMapping.cc @@ -757,15 +757,6 @@ bool LocalMapping::TryInitVIO(void) KeyFrame* pKF = lpKFtoCheck.front(); const set sChilds = pKF->GetChilds(); cv::Mat Twc = pKF->GetPoseInverse(); - const NavState& NS = pKF->GetNavState(); - //Debug log - if(pKF->mnBAGlobalForKF==nGBAKF) - { - cv::Mat tTwb1 = Twc*ConfigParam::GetMatT_cb(); - if((Converter::toVector3d(tTwb1.rowRange(0,3).col(3))-NS.Get_P()).norm()>1e-6) - cout<<"Twc*Tcb != NavState for GBA KFs, id "<mnId<<": "<mnBAGlobalForKF != nGBAKF???"<::const_iterator sit=sChilds.begin();sit!=sChilds.end();sit++) { KeyFrame* pChild = *sit; @@ -799,11 +790,6 @@ bool LocalMapping::TryInitVIO(void) lpKFtoCheck.pop_front(); - //Test log - cv::Mat tTwb = pKF->GetPoseInverse()*ConfigParam::GetMatT_cb(); - Vector3d tPwb = Converter::toVector3d(tTwb.rowRange(0,3).col(3)); - if( (tPwb-pKF->GetNavState().Get_P()).norm()>1e-6 ) - cerr<<"pKF PoseInverse Pwb != NavState.P ?"<GetNavState().Get_P().transpose()< &lLoc Vector3d Pref; Pref << refXY[0], refXY[1], 1.0; double rho = vPoint->estimate(); - if(rho<1e-4) {cerr<<"rho="<GetFirstVINSInited() && !bMapUpdated) cerr<<"1-FirstVinsInit, but not bMapUpdated. shouldn't"<GetFirstVINSInited() && !bMapUpdated) cerr<<"1-FirstVinsInit, but not bMapUpdated. shouldn't"< 1e-6) cerr<<"TrackLocalMapWithIMU current Frame dBias acc not zero"< 1e-6) cerr<<"TrackLocalMapWithIMU current Frame dBias gyr not zero"<GetFirstVINSInited() || bMapUpdated) { - if(mpLocalMapper->GetFirstVINSInited() && !bMapUpdated) cerr<<"2-FirstVinsInit, but not bMapUpdated. shouldn't"<GetFirstVINSInited() && !bMapUpdated) cerr<<"2-FirstVinsInit, but not bMapUpdated. shouldn't"<