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