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);

};
示例#4
0
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));
}