void VisionEngine::mainLoop()
{
unsigned char* cameraReadBuffer = NULL;
while(running_) {
// do nothing if paused
if (pause_){
interface_->processEvents();
pv_sleep();
continue;
}
//long start_time = currentMicroSeconds();
cameraReadBuffer = ringBuffer->getNextBufferToRead();
// loop until we get access to a frame
while (cameraReadBuffer==NULL) {
interface_->processEvents();
if (!running_) {
if(error_) interface_->displayError("Camera disconnected!");
return;
}
pv_sleep();
cameraReadBuffer = ringBuffer->getNextBufferToRead();
//if (cameraReadBuffer!=NULL) break;
}
//long camera_time = currentMicroSeconds()-start_time;
// do the actual image processing job
for (frame = processorList.begin(); frame!=processorList.end(); frame++)
(*frame)->process(cameraReadBuffer,destBuffer_);
//long processing_time = currentMicroSeconds()-start_time;
if (interface_->getDisplayMode()==SOURCE_DISPLAY)
memcpy(sourceBuffer_,cameraReadBuffer,ringBuffer->size());
ringBuffer->readFinished();
#ifndef NDEBUG
if (recording_) {
if (interface_->getDisplayMode()==SOURCE_DISPLAY)
saveBuffer(sourceBuffer_,format_);
else saveBuffer(destBuffer_,format_);
}
#endif
if (running_) {
if (camera_) camera_->showInterface(interface_);
interface_->updateDisplay();
}
//long total_time = currentMicroSeconds()-start_time;
//frameStatistics(camera_time,processing_time,total_time);
}
}
GLBox()
{
// Create the main window
sf::Window window(sf::VideoMode(RESOLUTION, RESOLUTION, 32), "program1");
graphicsInit();
unsigned int frameCount = 0;
// Start render loop
while (window.isOpen())
{
// Set the active window before using OpenGL commands
// It's not needed here because the active window is always the same,
// but don't forget it if you use multiple windows or controls
window.setActive();
// Handle any events that are in the queue
sf::Event event;
while (window.pollEvent(event))
{
// Close window : exit
//if (event.type == sf::Event::Closed)
// window.close();
// Escape key : exit
if ((event.type == sf::Event::KeyPressed) && (event.key.code == sf::Keyboard::Escape))
window.close();
// This is for grading your code. DO NOT REMOVE
if(event.type == sf::Event::KeyPressed && event.key.code == sf::Keyboard::Equal)
{
saveBuffer(window);
}
}
// Render the scene
display();
frameCount++;
if(frameCount == 100)
saveBuffer(window);
// Finally, display rendered frame on screen
window.display();
}
}
bool BackupDevice::import_duc(const char* filename, u32 force_size)
{
size_t elements_read;
u32 size;
char id[16];
FILE* file = fopen(filename, "rb");
if(!file) return false;
fseek(file, 0, SEEK_END);
size = (u32)ftell(file) - 500;
fseek(file, 0, SEEK_SET);
// Make sure we really have the right file
elements_read = fread((void *)id, sizeof(char), 16, file);
if (elements_read != 16)
printf("DUC file should be 16 bytes, not %lu bytes.\n", elements_read);
if (memcmp(id, "ARDS000000000001", 16) != 0)
{
printf("Not recognized as a valid DUC file\n");
fclose(file);
return false;
}
// Skip the rest of the header since we don't need it
fseek(file, 500, SEEK_SET);
u32 left = 0;
if (force_size > 0)
{
if (size > force_size)
size = force_size;
else
if (size < force_size)
{
left = force_size - size;
size = force_size;
}
}
raw_applyUserSettings(size, (force_size > 0));
u8 *data = new u8[size];
u32 sz = (size-left);
bool res = (fread(data, 1, sz, file) == sz);
fclose(file);
if (res)
saveBuffer(data, sz, true, true);
delete [] data;
return res;
}
void VisionEngine::event(int key)
{
if( key == KEY_O ){
display_lock_ = camera_->showSettingsDialog(display_lock_);
}
#ifndef NDEBUG
else if( key == KEY_M ){
recording_ = !recording_;
} else if( key == KEY_L ){
if (interface_->getDisplayMode()==SOURCE_DISPLAY)
saveBuffer(sourceBuffer_,format_);
else saveBuffer(destBuffer_,format_);
}
#endif
//printf("%d\n",key);
if (camera_) camera_->control(key);
for (frame = processorList.begin(); frame!=processorList.end(); frame++)
display_lock_ = (*frame)->toggleFlag(key,display_lock_);
}
std::string Camera::shoot()
{
if (pslr_shutter(theHandle) != PSLR_OK)
{
DPRINT("Did not shoot.");
return "";
};
std::string fn = getFilename();
while (!saveBuffer(fn))
usleep(10000);
deleteBuffer();
DPRINT("Shot.");
return lastFilename;
}
bool BackupDevice::import_no_gba(const char *fname, u32 force_size)
{
FILE *fsrc = fopen(fname, "rb");
u8 *in_buf = NULL;
u8 *out_buf = NULL;
if (fsrc)
{
u32 fsize = 0;
fseek(fsrc, 0, SEEK_END);
fsize = ftell(fsrc);
fseek(fsrc, 0, SEEK_SET);
//printf("Open %s file (size %i bytes)\n", fname, fsize);
in_buf = new u8 [fsize];
if (fread(in_buf, 1, fsize, fsrc) == fsize)
{
out_buf = new u8 [8 * 1024 * 1024 / 8];
u32 size = 0;
memset(out_buf, 0xFF, 8 * 1024 * 1024 / 8);
if (no_gba_unpackSAV(in_buf, fsize, out_buf, size) == 0)
{
if (force_size > 0)
size = force_size;
//printf("New size %i byte(s)\n", size);
size = trim(out_buf, size);
//printf("--- new size after trim %i byte(s)\n", size);
size = fillLeft(size);
//printf("--- new size after fill %i byte(s)\n", size);
raw_applyUserSettings(size, (force_size > 0));
saveBuffer(out_buf, size, true, true);
if (in_buf) delete [] in_buf;
if (out_buf) delete [] out_buf;
fclose(fsrc);
return true;
}
if (out_buf) delete [] out_buf;
}
if (in_buf) delete [] in_buf;
fclose(fsrc);
}
return false;
}
/* Handshake */
int spidy_main(void *unused)
{
int found=0, i=0;
/* Gpio initialization */
gpio_init();
/* MOD GPIO DIR AF */
for (i=0; i<18; i++)
gpio_dir_af(used_gpio[i], 1, 0, 0);
i=0;
puts("Your friendly neighborhood Spider-Man\n");
/* FIXME: spidy_reset(); */
/* Handshake */
while (!found) {
putc(CHAR_MSTART);
/* read from RBR start character */
if ( getCommand() == CHAR_SSTART)
found=1;
}
putc(CHAR_MREADY);
/* Take at least 18 commands --> BLOCKING */
while (i<18) {
/* ready to get buffer */
while (!getBuffer())
;
/* if a buffer has been caught send a ready */
i += saveBuffer();
putc(CHAR_MREADY);
}
frameIndex=0;
portNow=0;
portLast=-1;
return 0;
}
bool BackupDevice::import_raw(const char* filename, u32 force_size)
{
FILE* inf = fopen(filename,"rb");
if (!inf) return false;
fseek(inf, 0, SEEK_END);
u32 size = (u32)ftell(inf);
u32 left = 0;
if (force_size > 0)
{
if (size > force_size)
size = force_size;
else
if (size < force_size)
{
left = force_size - size;
size = force_size;
}
}
fseek(inf, 0, SEEK_SET);
raw_applyUserSettings(size, (force_size > 0));
u8 *data = new u8[size];
u32 sz = (size-left);
bool res = (fread(data, 1, sz, inf) == sz);
fclose(inf);
if (res)
saveBuffer(data, sz, true, true);
delete [] data;
return true;
}
/*
*
* 获取一帧数据
*
* */
static int read_frame(int fd)
{
struct v4l2_buffer buf;
memset(&buf,0,sizeof(buf));
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
if(-1 == ioctl(fd,VIDIOC_DQBUF,&buf))
{
MY_LOG("VIDIOC_DQBUF failed!\n");
}
current_frame = buf.index;
saveBuffer(buf);
if(-1 == ioctl(fd,VIDIOC_QBUF,&buf))
{
MY_LOG("VIDIOC_QBUF failed!\n");
}
return 0;
}
/* In boot.S there is a loop of spidy_step */
void spidy_step(void *unused)
{
/* Wait till next beginning */
while (regs[REG_TMR32B1PC] >= 30)
;
/* up motor i, frameIndex != 9 && != 19 */
if (portNow >= 0)
gpio_dir(used_gpio[portNow], 1, 1);
/* down motor i-1 if it's > 52 deg, frameIndex != 0 && != 10 */
if (portLast >= 0) {
if (motors[portLast] > DEGREELIMIT) {
waitTime = TIMEDEG * (motors[portLast] - DEGREELIMIT);
while (regs[REG_TMR32B1PC] < waitTime)
;
gpio_dir(used_gpio[portLast], 1, 0);
}
} else {
/* acquiring buffer */
if (getBuffer()) {
saveBuffer();
putc(CHAR_MREADY); /* to get next burst */
}
}
/* down motor i if it's <= 52 deg, frameIndex != 9 && != 19 */
if (portNow >= 0) {
if (motors[portNow] <= DEGREELIMIT) {
waitTime = (TIMEDEG * (motors[portNow])) + TIME090;
while(regs[REG_TMR32B1PC] < waitTime)
;
gpio_dir(used_gpio[portNow], 1, 0);
}
}
/* setup next motor */
setPort();
}
void saveObject( const DataBufferObject& obj, int fd ) throw (MC2String) {
DataBuffer buff( obj.getSizeInDataBuffer() );
obj.save( buff );
saveBuffer( buff, fd );
}
void PortVideoSDL::process_events()
{
SDL_Event event;
int cameraFlux=0; //THis is wheter the camera is in flux. while it is we need to not use camera_ as it is intedeterminated
while( SDL_PollEvent( &event ) ) {
switch( event.type ) {
case SDL_USEREVENT:
{
switch(event.user.code){
case CAMERA_OFF:
SDL_mutexP(controlMutex_);
if(pause_==false){
pause_=true;
SDL_CondWait(controlCond_,controlMutex_);
}
SDL_mutexV(controlMutex_);
break;
case CAMERA_ON:
SDL_mutexP(controlMutex_);
if(pause_==true){
pause_=false;
SDL_CondWait(controlCond_,controlMutex_);
}
SDL_mutexV(controlMutex_);
//messageServer_->add
break;
/*
case CAMERA_OFF_COMPLETE:
//Broadcast the camera is off.
break;
case CAMERA_ON_COMPLETE:
break;
*/
}
break;
}
case SDL_KEYDOWN:
if (error_) { error_ = false; return; }
//printf("%d\n",event.key.keysym.sym);
if( event.key.keysym.sym == SDLK_n ){
displayMode_ = NO_DISPLAY;
// turn the display black
SDL_FillRect(window_,0,0);
SDL_Flip(window_);
} else if( event.key.keysym.sym == SDLK_s ){
SDL_FillRect(displayImage_, NULL, 0 );
displayMode_ = SOURCE_DISPLAY;
} else if( event.key.keysym.sym == SDLK_t ){
SDL_FillRect(displayImage_, NULL, 0 );
displayMode_ = DEST_DISPLAY;
} else if( event.key.keysym.sym == SDLK_o ){
camera_->showSettingsDialog();
} else if( event.key.keysym.sym == SDLK_v ){
if (verbose_) {
verbose_=false;
} else {
help_ = false;
verbose_=true;
}
}
#ifndef NDEBUG
else if( event.key.keysym.sym == SDLK_m ){
if (recording_) {
recording_ = false;
char caption[24] = "";
sprintf(caption,"%s - %d FPS",app_name_.c_str(),current_fps);
SDL_WM_SetCaption( caption, NULL );
} else {
struct stat info;
#ifdef WIN32
if (stat(".\recording",&info)!=0) {
std::string dir(".\recording");
LPSECURITY_ATTRIBUTES attr = NULL;
CreateDirectory(dir.c_str(),attr);
}
#else
if (stat("./recording",&info)!=0) mkdir("./recording",0777);
#endif
recording_ = true;
std::string caption = app_name_ + " - recording";
SDL_WM_SetCaption( caption.c_str(), NULL );
}
} else if( event.key.keysym.sym == SDLK_b ){
struct stat info;
#ifdef WIN32
if (stat(".\recording",&info)!=0) {
std::string dir(".\recording");
LPSECURITY_ATTRIBUTES attr = NULL;
CreateDirectory(dir.c_str(),attr);
}
#else
if (stat("./recording",&info)!=0) mkdir("./recording",0777);
#endif
if (displayMode_==DEST_DISPLAY) saveBuffer(destBuffer_);
else saveBuffer(sourceBuffer_);
}
#endif
else if( event.key.keysym.sym == SDLK_p ) {
SDL_mutexP(controlMutex_);
if (pause_) {
pause_=false;
char caption[24] = "";
sprintf(caption,"%s - %d FPS",app_name_.c_str(),current_fps);
SDL_WM_SetCaption( caption, NULL );
} else {
pause_=true;
std::string caption = app_name_ + " - paused";
SDL_WM_SetCaption( caption.c_str(), NULL );
// turn the display black
SDL_FillRect(window_,0,0);
SDL_Flip(window_);
}
SDL_CondWait(controlCond_,controlMutex_);
SDL_mutexV(controlMutex_);
} else if( event.key.keysym.sym == SDLK_c ) {
if (calibrate_) {
calibrate_=false;
char caption[24] = "";
sprintf(caption,"%s - %d FPS",app_name_.c_str(),current_fps);
SDL_WM_SetCaption( caption, NULL );
} else {
calibrate_=true;
std::string caption = app_name_ + " - calibration";
SDL_WM_SetCaption( caption.c_str(), NULL );
}
} else if( event.key.keysym.sym == SDLK_h ){
help_=!help_;
verbose_=false;
} else if( event.key.keysym.sym == SDLK_ESCAPE ){
running_=false;
return;
}
for (frame = processorList.begin(); frame!=processorList.end(); frame++)
(*frame)->toggleFlag(event.key.keysym.sym);
if(camera_)camera_->control(event.key.keysym.sym);
break;
case SDL_QUIT:
running_ = false;
error_ = false;
break;
}
}
}
// does what its name suggests
void PortVideoSDL::mainLoop()
{
unsigned char* cameraReadBuffer = NULL;
while(running_) {
// do nothing if paused
if (pause_){
process_events();
SDL_Delay(50);
continue;
}
long start_time = currentTime();
cameraReadBuffer = ringBuffer->getNextBufferToRead();
// loop until we get access to a frame
while (cameraReadBuffer==NULL) {
cameraReadBuffer = ringBuffer->getNextBufferToRead();
//if (cameraReadBuffer!=NULL) break;
SDL_Delay(1);
process_events();
if(!running_) {
endLoop();
return;
}
}
long camera_time = currentTime();
//memcpy(sourceBuffer_,cameraReadBuffer,ringBuffer->size());
//ringBuffer->readFinished();
// try again if we can get a more recent frame
/*do {
memcpy(sourceBuffer_,cameraReadBuffer,ringBuffer->size());
ringBuffer->readFinished();
cameraReadBuffer = ringBuffer->getNextBufferToRead();
} while( cameraReadBuffer != NULL );*/
// do the actual image processing job
for (frame = processorList.begin(); frame!=processorList.end(); frame++)
(*frame)->process(cameraReadBuffer,destBuffer_,displayImage_);
long processing_time = currentTime();
if(camera_ != 0){
// update display
switch( displayMode_ ) {
case NO_DISPLAY:
break;
case SOURCE_DISPLAY: {
memcpy(sourceBuffer_,cameraReadBuffer,ringBuffer->size());
SDL_BlitSurface(sourceImage_, NULL, window_, NULL);
if (help_) drawHelp();
camera_->drawGUI(displayImage_);
SDL_BlitSurface(displayImage_, NULL, window_, NULL);
SDL_FillRect(displayImage_, NULL, 0 );
SDL_Flip(window_);
break;
}
case DEST_DISPLAY: {
SDL_BlitSurface(destImage_, NULL, window_, NULL);
if (help_) drawHelp();
camera_->drawGUI(displayImage_);
SDL_BlitSurface(displayImage_, NULL, window_, NULL);
SDL_FillRect(displayImage_, NULL, 0 );
SDL_Flip(window_);
break;
}
}
}
#ifndef NDEBUG
if (recording_) {
if (displayMode_!=SOURCE_DISPLAY) memcpy(sourceBuffer_,cameraReadBuffer,ringBuffer->size());
saveBuffer(sourceBuffer_);
}
#endif
ringBuffer->readFinished();
if (!recording_) frameStatistics(camera_time-start_time,processing_time-camera_time, currentTime()-start_time);
process_events();
}
endLoop();
}
void SilhouetteExtractor::computeVisibleFrontFacingStatus()
{
int terrain_width = terrain_->width();
int terrain_height = terrain_->height();
delete front_facing_;
front_facing_ = new FacingMode [(terrain_width-1)*(terrain_height-1)];
bool use_intersections = true ;
if (!use_intersections)
{
setupPixelBuffer();
pixelbuffer_->makeCurrent();
glEnable(GL_DEPTH_TEST);
glEnable(GL_LIGHT0);
glEnable(GL_LIGHTING);
GLfloat lightpos[] = {.5, 1., 1., 0.};
glLightfv(GL_LIGHT0, GL_POSITION, lightpos);
glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
drawTerrain();
updateMatrices();
saveBuffer("test_dpth.png");
pixelbuffer_->makeCurrent();
}
double begin = omp_get_wtime();
int i = 0;
#pragma omp parallel for private(i)
for (int j = 0; j < terrain_->height()-1; ++j)
for (i = 0; i < terrain_->width()-1; ++i)
{
front_facing_[j*(terrain_width-1)+i] = kInvisible;
Eigen::Vector3f center = getFaceCentroid(i, j);
Eigen::Vector3f projector = center - camera_info_.position;
//projector = camera_info_.direction;
float theta = acos(camera_info_.direction.normalized().dot(projector.normalized()));
if (theta > camera_info_.fov_in_rads/2)
continue;
front_facing_[j*(terrain_width-1)+i] = kBackFacing;
if (terrain_->getGridNormal(i, j).dot(projector) <= -FLT_EPSILON)
{
if (use_intersections)
{
if (checkVisibility(center))
front_facing_[j*(terrain_width-1)+i] = kFrontFacing;
} else
{
Eigen::Vector3d window_coords;
gluProject(center[0], center[1], center[2],
modelview_matrix_, projection_matrix_, viewport_,
&window_coords[0], &window_coords[1], &window_coords[2]);
if (window_coords[0] < 0 || window_coords[1] < 0 || window_coords[0] >= width() || window_coords[1] >= height())
continue;
float depth = 0.0;
glReadPixels(window_coords[0], window_coords[1], 1, 1, GL_DEPTH_COMPONENT, GL_FLOAT, &depth);
if (std::abs(depth-window_coords[2]) < 1e-3)
front_facing_[j*(terrain_width-1)+i] = kFrontFacing;
}
}
}
double end = omp_get_wtime();
double elapsed_secs = double(end - begin);
fprintf(stdout, "Elapsed time for checking front/back facing: %.2f secs\n", elapsed_secs);
fprintf(stdout, "Num of threads: %d threads\n", omp_get_thread_num());
fflush(stdout);
if (pixelbuffer_)
{
pixelbuffer_->doneCurrent();
cleanupPixelBuffer();
}
}
