void TYSmoothFilter::reset(){ tbHead = -1; print_count = 0; start_count = 0; prePose = Vec6f(0.f, 0.f, 0.f, 0.f, 0.f, 0.f); curPose = Vec6f(0.f, 0.f, 0.f, 0.f, 0.f, 0.f); for(int i = 0 ; i < smoothFrameNum ; i++) HTB[i] = Vec6f(0.f, 0.f, 0.f, 0.f, 0.f, 0.f); }
TYSmoothFilter::TYSmoothFilter(int filterIndex){ smoothFrameNum = SMOOTH_FRAME_NUM; tbHead = -1; print_count = 0; start_count = 0; fIndex = filterIndex; regRawInHTB = true; prePose = Vec6f(0.f, 0.f, 0.f, 0.f, 0.f, 0.f); curPose = Vec6f(0.f, 0.f, 0.f, 0.f, 0.f, 0.f); for(int i = 0 ; i < smoothFrameNum ; i++) HTB[i] = Vec6f(0.f, 0.f, 0.f, 0.f, 0.f, 0.f); }
/* Constructor */ TYCloudMatcher::TYCloudMatcher(){ gradient.push_back( Vec6f( 0.f , 0.f , 0.f , 0.f , 0.f , 0.f ) ); gradient.push_back( Vec6f( 1.f , 0.f , 0.f , 0.f , 0.f , 0.f ) ); gradient.push_back( Vec6f( -1.f, 0.f , 0.f , 0.f , 0.f , 0.f ) ); gradient.push_back( Vec6f( 0.f , 1.f , 0.f , 0.f , 0.f , 0.f ) ); gradient.push_back( Vec6f( 0.f , -1.f, 0.f , 0.f , 0.f , 0.f ) ); gradient.push_back( Vec6f( 0.f , 0.f , 1.f , 0.f , 0.f , 0.f ) ); gradient.push_back( Vec6f( 0.f , 0.f , -1.f, 0.f , 0.f , 0.f ) ); gradient.push_back( Vec6f( 0.f , 0.f , 0.f , 1.f , 0.f , 0.f ) ); gradient.push_back( Vec6f( 0.f , 0.f , 0.f , -1.f, 0.f , 0.f ) ); gradient.push_back( Vec6f( 0.f , 0.f , 0.f , 0.f , 1.f , 0.f ) ); gradient.push_back( Vec6f( 0.f , 0.f , 0.f , 0.f , -1.f, 0.f ) ); gradient.push_back( Vec6f( 0.f , 0.f , 0.f , 0.f , 0.f , 1.f ) ); gradient.push_back( Vec6f( 0.f , 0.f , 0.f , 0.f , 0.f , -1.f) ); rStep = ROTATE_STEP; tStep = TRANSLATE_STEP; cvgCtr = 0; isConverged = true; cvgThreshC = CVG_THRE_CNT; cvgThreshR = CVG_THRE_ROT; cvgThreshEPP = CVG_THRE_EPP; cvgThreshGoEPP = CVG_THRE_GoEPP; query = flann::Matrix<float>(qData, 1, 3); };
Range6<float> Range6<float>::Empty() { return Range6<float>(Vec6f(FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX), Vec6f(-FLT_MAX, -FLT_MAX, -FLT_MAX, -FLT_MAX, -FLT_MAX, -FLT_MAX)); }