C++ LTimer::stop方法代码示例

Mat icp_outlierRemoval(Pixel3DSet & src, Pixel3DSet & _dst, Pixel3DSet & out,
					   double & error_out, double & time, int & iterations,
					   double outlierRemovalTimesMean, double minError,
					   int maxIterations)
{
	time = 0.0;
	LTimer clock; clock.start();
	out.clear();
	Mat ret = Mat::eye(Size(4,4), CV_32FC1);
	vector<double> dl;
	vector<int> ind;
	out = src;
	Pixel3DSet dst = _dst;
	iterations = 0;

	double best_distance = DBL_MAX;
	error_out = best_distance;

	Mat T = ret.clone();

	while(true)
	{
		dl.clear(); ind.clear();
		double current_distance = Licp::closestPointsf(out, dst, ind, dl);
		if(current_distance >= best_distance) break; //cannot do any better
		ret = T * ret;
		error_out = current_distance;
		if(minError > 0.0 && current_distance < minError) break;
		if(maxIterations >= 0 && iterations >= maxIterations) break;
		best_distance = current_distance;
		if(best_distance <= 0.0) break;

		//outlier removal
		//cout << current_distance << " is current, or is: " << outlierRemovalTimesMean << endl;
		vector<R3> srcTest, dstTest;
		vector<int> indTest;
		for(int i = 0; i < dl.size(); i++)
		{
			double test = dl[i] / best_distance;
			//cout << dl[i] << " : " << best_distance << " : " << test << " <= " << outlierRemovalTimesMean << endl;
			if(dl[i]/best_distance < outlierRemovalTimesMean)
			{
				srcTest.push_back(out[i]);
				dstTest.push_back(dst[ind[i]]);
				indTest.push_back(i);
			}
		}

		//cout << "i/o(" << out.size() << "/" << srcTest.size() << ")\n";

		T = Licp::leastSquaresTransform(srcTest, dstTest, indTest);
		for(int i = 0; i < out.size(); i++) ll_pix3d::Pixel3DSet::transform_point(T, out[i]);
		iterations++;
	}
	clock.stop();
	time = clock.getSeconds();
	return ret.clone();
}

void startStopTimer()
{
    if (gTimer.isStarted())
    {
        gTimer.stop();
    }
    else
    {
        gTimer.start();
    }
}

Mat icp(Pixel3DSet & src, Pixel3DSet & dst, Pixel3DSet & out,
		double & error_out, double & time, int & iterations,
		double minError, int maxIterations)
{
	time = 0.0;
	LTimer clock; clock.start();
	out.clear();
	Mat ret = Mat::eye(Size(4,4), CV_32FC1);
	vector<double> dl;
	vector<int> ind;
	out = src;
	iterations = 0;

	double best_distance = DBL_MAX;
	error_out = best_distance;

	Mat T = ret.clone();

	while(true)
	{
		dl.clear(); ind.clear();
		double current_distance = closestPointsf(out, dst, ind, dl);
		if(current_distance >= best_distance) break; //cannot do any better
		ret = T * ret;
		error_out = current_distance;
		if(minError > 0.0 && current_distance < minError) break;
		if(maxIterations >= 0 && iterations >= maxIterations) break;
		best_distance = current_distance;
		T = leastSquaresTransform(out.points, dst.points, ind);
		for(int i = 0; i < out.size(); i++) ll_pix3d::Pixel3DSet::transform_point(T, out[i]);
		iterations++;
	}
	clock.stop();
	time = clock.getSeconds();
	return ret.clone();
}

int main( int argc, char* args[] )
{
	//Start up SDL and create window
	if( !init() )
	{
		printf( "Failed to initialize!\n" );
	}
	else
	{
		//Load media
		if( !loadMedia() )
		{
			printf( "Failed to load media!\n" );
		}
		else
		{	
			//Main loop flag
			bool quit = false;

			//Event handler
			SDL_Event e;

			//Set text color as black
			SDL_Color textColor = { 0, 0, 0, 255 };

			//The application timer
			LTimer timer;

			//In memory text stream
			std::stringstream timeText;

			//While application is running
			while( !quit )
			{
				//Handle events on queue
				while( SDL_PollEvent( &e ) != 0 )
				{
					//User requests quit
					if( e.type == SDL_QUIT )
					{
						quit = true;
					}
					//Reset start time on return keypress
					else if( e.type == SDL_KEYDOWN )
					{
						//Start/stop
						if( e.key.keysym.sym == SDLK_s )
						{
							if( timer.isStarted() )
							{
								timer.stop();
							}
							else
							{
								timer.start();
							}
						}
						//Pause/unpause
						else if( e.key.keysym.sym == SDLK_p )
						{
							if( timer.isPaused() )
							{
								timer.unpause();
							}
							else
							{
								timer.pause();
							}
						}
					}
				}

				//Set text to be rendered
				timeText.str( "" );
				timeText << "Seconds since start time " << ( timer.getTicks() / 1000.f ) ; 

				//Render text
				if( !gTimeTextTexture.loadFromRenderedText( timeText.str().c_str(), textColor ) )
				{
					printf( "Unable to render time texture!\n" );
				}

				//Clear screen
				SDL_SetRenderDrawColor( gRenderer, 0xFF, 0xFF, 0xFF, 0xFF );
				SDL_RenderClear( gRenderer );

				//Render textures
				gStartPromptTexture.render( ( SCREEN_WIDTH - gStartPromptTexture.getWidth() ) / 2, 0 );
				gPausePromptTexture.render( ( SCREEN_WIDTH - gPausePromptTexture.getWidth() ) / 2, gStartPromptTexture.getHeight() );
				gTimeTextTexture.render( ( SCREEN_WIDTH - gTimeTextTexture.getWidth() ) / 2, ( SCREEN_HEIGHT - gTimeTextTexture.getHeight() ) / 2 );

				//Update screen
				SDL_RenderPresent( gRenderer );
			}
		}
	}

	//Free resources and close SDL
	close();

//.........这里部分代码省略.........