void lidarlocalizorapp(){
  printf("starting\n");
 
  Lidar lidar;
  lidar.init("lidar/lidarconfig.conf");
  
  LidarManager lidarManager;
  lidarManager.lidar=&lidar; 
  
  lidarManager.init("lidar/lidarpositionconfig.conf");
  
  
  LidarLocalizer localizer;
  localizer.loadPresetMap();
  
  WorldModel model;
  model.init("indoorlidarconf.conf");
  model.lidar=&lidarManager;
  model.lidarLocalizer=&localizer;
  
  Network network;  
  model.network=&network;
  
	Display display=Display(&model);
  
  //start timer
  TimeKeeper::start_time();

  printf("made all objects\n");
  int key = 0;	
  long start=0;
  double x;
  double y;
  try {
	while((key&255)!=27){

	  start=TimeKeeper::GetTime();
	 // printf("loop start\n");
	  
	  model.updateModel();
	//  localizer.estimatePosition(&state,&scanpoints,fake_scan_point_count);

	//  printf("model updated\n");
	  //display.displayModel();
      key=cvWaitKey(15);
	  
	  cout<<"position "<<model.state.crioPosition.x<<" "<<model.state.crioPosition.y<<" "<<model.state.crioRot<<endl;
	  cout<<"time elapsed "<<(TimeKeeper::GetTime()-start)/1000000.0<<endl;
	
	}
  }
  catch(...) {
	cerr << "we are catching an uncaught exception. bad things happened" << endl;
  }
  
}
Exemple #2
0
void recorder(){

	ofstream logfile;
	logfile.open("logfile.txt");

	ofstream lidarLogfile;
	lidarLogfile.open("lidarlogfile.txt");
	
	Lidar lidar;
	lidar.init("lidar/lidarconfig.conf");
	LidarManager lidarManager;
	lidarManager.lidar=&lidar;
	lidarManager.init("lidar/lidarpositionconfig.conf");

	WorldModel model;
	model.init("visionworldmodelconfig.conf");
	model.lidar=&lidarManager;
	
	Network network;
	
	model.network=&network;
	
   char buffer [50];
	Camera a;
   	a.init(0);
	a.loadCalib("vision/calibration/Intrinsic.xml","vision/calibration/Distortion.xml","vision/calibration/H.xml");
	CameraManager manager;
   	manager.init();
   	manager.addCam(&a);
   	manager.startThread();
   	VisionInterpreter vision= VisionInterpreter(&manager);
   	vision.init("vision/visioninterpreter.conf");
   	vision.manager=&manager;
   	model.vision=&vision;

	Display display=Display(&model);

	//start timer
	TimeKeeper::start_time();

	printf("made all objects\n");
	int key = 0;	
	long start=0;
	try {
		while((key&255)!=27){
			start=TimeKeeper::GetTime();
			model.updateModel();
			display.displayModel();
			key=cvWaitKey(3);

			//write a timestamp to the logfile
			logfile<<start<<endl;
			//write the crio state to the logfile
			logfile<<model.state.crioPosition.x<<" "<<model.state.crioPosition.y<<" "<<model.state.crioRotRad<<endl;
			//write the lidar data to the logfile
			
			lidarLogfile<<start<<endl;
			lidarLogfile<<lidar.num_values<<endl;
			for(int i=0;i<lidar.num_values;i++){
				lidarLogfile<<lidar.values[i]<<" ";
			}
			lidarLogfile<<endl;
		
			sprintf (buffer, "output/output%.7ld.bmp",start);
			if(!cvSaveImage(buffer,a.image,0)) printf("Could not save: %s\n",buffer);
			else printf("picture taken!!!\n");
			cout<<model.state.crioPosition.x<<" "<<model.state.crioPosition.y<<" "<<model.state.crioRotRad<<endl;
			cout<<TimeKeeper::GetTime()-start<<endl;
			
			if((key&255) ==32){
				if(!cvSaveImage("output/vision map.jpg",model.worldModelVision,0)) printf("Could not save");
				else printf("picture taken!!!\n");
		  }
		}
	}
	catch(...) {
    cerr << "we are catching an uncaught exception. bad things happened" << endl;
  }
  lidarLogfile.close();
	logfile.close();
}
void pointandclick(){

	Lidar lidar;
	lidar.init("lidar/lidarconfig.conf");
	LidarManager lidarManager;
	lidarManager.lidar=&lidar;
	lidarManager.init("lidar/lidarpositionconfig.conf");

	WorldModel model;
	model.init("visionworldmodelconfig.conf");
	model.lidar=&lidarManager;
	
	Network network;
	
	model.network=&network;
	printf("made network\n");
	
	Camera a;
   	a.init(0);
	a.loadCalib("vision/calibration/Intrinsic.xml","vision/calibration/Distortion.xml","vision/calibration/H.xml");
	CameraManager manager;
   	manager.init();
   	manager.addCam(&a);
   	manager.startThread();
   	
	printf("made camera\n");
	
   	VisionInterpreter vision= VisionInterpreter(&manager);
   	printf("pre init\n");
   	vision.init("vision/visioninterpreter.conf");
   	printf("post init\n");
   	vision.manager=&manager;
   	model.vision=&vision;
   	
	printf("made vision interpreter\n");
	
	PointAndClickDisplay display=PointAndClickDisplay(&model);

	printf("made all objects\n");
	//start timer
	TimeKeeper::start_time();
	int key = 0;	
	long start=0;
	try {
		while((key&255)!=27){
			start=TimeKeeper::GetTime();
			cout<<"update model\n";
			model.updateModel();
			cout<<"update disp\n";
			display.displayModel();
			key=cvWaitKey(3);
			  
		}
			
		//	cout<<model.state.crioPosition.x<<" "<<model.state.crioPosition.y<<" "<<model.state.crioRot<<endl;
	//		cout<<TimeKeeper::GetTime()-start<<endl;
	}
	catch(...) {
    cerr << "we are catching an uncaught exception. bad things happened" << endl;
  }
}