int main(int argc,char* argv[]) { LidarProcessOctree lpo(argv[1],512*1024*1024); #if 0 Misc::Timer t; { // BinaryPointSaver bps(argv[2]); double scale[3]={0.001,0.001,0.001}; double offset[3]; for(int i=0;i<3;++i) offset[i]=lpo.getDomain().getCenter(i); LasPointSaver lps(argv[2],scale,offset); // PointCounter pc; Box box(Box::Point(2.04446e6,617053.0,-1000.0),Box::Point(2.04446e6+100.0,617053.0+100.0,1000.0)); lpo.processPointsInBox(box,lps); // std::cout<<lps.getNumPoints()<<std::endl; } t.elapse(); std::cout<<"Done in "<<t.getTime()*1000.0<<" ms"<<std::endl; #elif 0 PointSaver ps(argv[2]); Box box=Box::full; for(int i=0;i<2;++i) { box.min[i]=atof(argv[3+i]); box.max[i]=atof(argv[5+i]); } lpo.processPointsInBox(box,ps); #elif 0 PointCounter pc; lpo.processPoints(pc); std::cout<<pc.getNumPoints()<<std::endl; #else Scalar radius=Scalar(0.1); PointDensityCalculator pdc(lpo,radius,4); #if 0 Box box; box.min=Point(930,530,0); box.max=Point(970,570,100); lpo.processPointsInBox(box,pdc); #else // lpo.processPoints(pdc); lpo.processNodesPostfix(pdc); #endif std::cout<<"Number of processed points: "<<pdc.numPoints<<std::endl; std::cout<<"Total number of found neighbors: "<<pdc.totalNumNeighbors<<std::endl; std::cout<<"Total number of loaded octree nodes: "<<lpo.getNumSubdivideCalls()<<", "<<lpo.getNumLoadedNodes()<<std::endl; std::cout<<"Average point density in 1/m^3: "<<pdc.totalNumNeighbors/(pdc.numPoints*4.0/3.0*3.141592654*radius*radius*radius)<<std::endl; #endif return 0; }
void PointAccumulator::savePoints(void) { /* Create a temporary octree for the current in-memory point set: */ std::cout<<std::endl<<"Storing "<<points.size()<<" points as temporary octree..."<<std::flush; char tofnt[1024]; strcpy(tofnt,tempOctreeFileNameTemplate.c_str()); Misc::Timer t; TempOctree* to=new TempOctree(tofnt,maxNumPointsPerNode,&points[0],points.size()); t.elapse(); std::cout<<" done in "<<t.getTime()*1000.0<<" ms"<<std::endl; tempOctrees.push_back(to); /* Clear the point set: */ points.clear(); }
int main(int argc,char* argv[]) { LidarProcessOctree lpo(argv[1],512*1024*1024); Misc::Timer t; size_t numLeafPoints=0; for(LidarProcessOctree::PointIterator pIt=lpo.beginNodes();pIt!=lpo.endNodes();++pIt) ++numLeafPoints; t.elapse(); std::cout<<numLeafPoints<<" leaf points, "; std::cout<<"done in "<<t.getTime()*1000.0<<" ms"<<std::endl; return 0; }
int main(int argc,char* argv[]) { /* Parse command line: */ char* serverName=0; int trackerIndex=0; int printMode=0; bool printButtonStates=false; bool printNewlines=false; for(int i=1;i<argc;++i) { if(argv[i][0]=='-') { if(strcasecmp(argv[i],"-t")==0||strcasecmp(argv[i],"--trackerIndex")==0) { ++i; trackerIndex=atoi(argv[i]); } else if(strcasecmp(argv[i],"-alltrackers")==0) trackerIndex=-1; else if(strcasecmp(argv[i],"-p")==0) printMode=0; else if(strcasecmp(argv[i],"-o")==0) printMode=1; else if(strcasecmp(argv[i],"-f")==0) printMode=2; else if(strcasecmp(argv[i],"-v")==0) printMode=3; else if(strcasecmp(argv[i],"-b")==0) printButtonStates=true; else if(strcasecmp(argv[i],"-n")==0) printNewlines=true; } else serverName=argv[i]; } if(serverName==0) { std::cerr<<"Usage: "<<argv[0]<<" [(-t | --trackerIndex) <trackerIndex>] [-p | -o | -f | -v] [-b] <serverName:serverPort>"<<std::endl; return 1; } /* Initialize device client: */ Vrui::VRDeviceClient* deviceClient=0; try { /* Split the server name into hostname:port: */ char* colonPtr=0; for(char* cPtr=serverName;*cPtr!='\0';++cPtr) if(*cPtr==':') colonPtr=cPtr; int portNumber=0; if(colonPtr!=0) { portNumber=atoi(colonPtr+1); *colonPtr='\0'; } deviceClient=new Vrui::VRDeviceClient(serverName,portNumber); } catch(std::runtime_error error) { std::cerr<<"Caught exception "<<error.what()<<" while initializing VR device client"<<std::endl; return 1; } /* Print output header line: */ switch(printMode) { case 0: std::cout<<" Pos X Pos Y Pos Z "<<std::endl; break; case 1: std::cout<<" Pos X Pos Y Pos Z Axis X Axis Y Axis Z Angle "<<std::endl; break; case 2: std::cout<<" Pos X Pos Y Pos Z XA X XA Y XA Z YA X YA Y YA Z ZA X ZA Y ZA Z "<<std::endl; break; } /* Run main loop: */ deviceClient->activate(); deviceClient->startStream(); bool loop=true; Misc::Timer t; int numPackets=0; while(loop) { /* Print new device state: */ if(!printNewlines) std::cout<<"\r"; deviceClient->lockState(); const Vrui::VRDeviceState& state=deviceClient->getState(); switch(printMode) { case 0: if(trackerIndex<0) { printTrackerPos(state,0); for(int i=1;i<state.getNumTrackers();++i) { std::cout<<" "; printTrackerPos(state,i); } } else printTrackerPos(state,trackerIndex); break; case 1: printTrackerPosOrient(state,trackerIndex); break; case 2: printTrackerFrame(state,trackerIndex); break; case 3: printValuators(state); break; } if(printButtonStates) { std::cout<<" "; printButtons(state); } deviceClient->unlockState(); if(printNewlines) std::cout<<std::endl; else std::cout<<std::flush; /* Check for a key press event: */ fd_set readFdSet; FD_ZERO(&readFdSet); FD_SET(fileno(stdin),&readFdSet); struct timeval timeout; timeout.tv_sec=0; timeout.tv_usec=0; bool dataWaiting=select(fileno(stdin)+1,&readFdSet,0,0,&timeout)>=0&&FD_ISSET(fileno(stdin),&readFdSet); if(dataWaiting) loop=false; if(loop) { /* Wait for next packet: */ deviceClient->getPacket(); ++numPackets; } } std::cout<<std::endl; t.elapse(); std::cout<<"Received "<<numPackets<<" device data packets in "<<t.getTime()*1000.0<<" ms ("<<double(numPackets)/t.getTime()<<" packets/s)"<<std::endl; deviceClient->stopStream(); deviceClient->deactivate(); /* Clean up and terminate: */ delete deviceClient; return 0; }
int main(int argc,char* argv[]) { /* Set default values for all parameters: */ unsigned int memoryCacheSize=512; unsigned int tempOctreeMaxNumPointsPerNode=4096; std::string tempOctreeFileNameTemplate="/tmp/Pointe_Preprocessor_TempOctree"; unsigned int maxNumPointsPerNode=4096; std::string tempPointFileNameTemplate="/tmp/Pointe_Preprocessor_TempPoints"; const char * fileName=0; float valueMinimum=Math::Constants<float>::max; float valueMaximum=Math::Constants<float>::min; float * histogram; try { /* Open Pointe-Samhlaigh's configuration file: */ Misc::ConfigurationFile configFile(POINTE_CONFIGFILENAME); Misc::ConfigurationFileSection cfg=configFile.getSection("/Pointe_Preprocessor"); /* Override program settings from configuration file: */ memoryCacheSize=cfg.retrieveValue<unsigned int>("./memoryCacheSize",memoryCacheSize); tempOctreeMaxNumPointsPerNode=cfg.retrieveValue<unsigned int>("./tempOctreeMaxNumPointsPerNode",tempOctreeMaxNumPointsPerNode); tempOctreeFileNameTemplate=cfg.retrieveValue<std::string>("./tempOctreeFileNameTemplate",tempOctreeFileNameTemplate); maxNumPointsPerNode=cfg.retrieveValue<unsigned int>("./maxNumPointsPerNode",maxNumPointsPerNode); tempPointFileNameTemplate=cfg.retrieveValue<std::string>("./tempPointFileNameTemplate",tempPointFileNameTemplate); } catch(std::runtime_error err) { /* Just ignore the error */ } /* Initialize transient parameters: */ const char* outputFileName=0; PointFileType pointFileType=XYZI; bool havePoints=false; /* Parse the command line and load all input files: */ Misc::Timer loadTimer; PointeAccumulator pa; pa.setMemorySize(memoryCacheSize,tempOctreeMaxNumPointsPerNode); pa.setTempOctreeFileNameTemplate(tempOctreeFileNameTemplate+"XXXXXX"); for(int i=1;i<argc;++i) { if(argv[i][0]=='-') { if(strcasecmp(argv[i]+1,"o")==0) { ++i; if(i<argc) outputFileName=argv[i]; else std::cerr<<"Dangling -o flag on command line"<<std::endl; } else if(strcasecmp(argv[i]+1,"np")==0) { ++i; if(i<argc) maxNumPointsPerNode=(unsigned int)(atoi(argv[i])); else std::cerr<<"Dangling -np flag on command line"<<std::endl; } else if(strcasecmp(argv[i]+1,"cs")==0) { ++i; if(i<argc) { memoryCacheSize=(unsigned int)(atoi(argv[i])); pa.setMemorySize(memoryCacheSize,tempOctreeMaxNumPointsPerNode); } else std::cerr<<"Dangling -cs flag on command line"<<std::endl; } else if(strcasecmp(argv[i]+1,"to")==0) { ++i; if(i<argc) { if(!havePoints) { tempOctreeFileNameTemplate=argv[i]; pa.setTempOctreeFileNameTemplate(tempOctreeFileNameTemplate+"XXXXXX"); } else std::cerr<<"Ignoring -to flag; must be specified before any input point sets are read"<<std::endl; } else std::cerr<<"Dangling -to flag on command line"<<std::endl; } else if(strcasecmp(argv[i]+1,"tp")==0) { ++i; if(i<argc) tempPointFileNameTemplate=argv[i]; else std::cerr<<"Dangling -tp flag on command line"<<std::endl; } else if(strcasecmp(argv[i]+1,"xyzi")==0) pointFileType=XYZI; else std::cerr<<"Unrecognized command line option "<<argv[i]<<std::endl; } else { PointFileType thisPointFileType=pointFileType; switch(thisPointFileType) { case XYZI: std::cout<<"Processing XYZI input file "<<argv[i]<<"..."<<std::flush; histogram = new float[256]; fileName=argv[i]; loadPointFileXyzi(pa, fileName, &valueMaximum, &valueMinimum); for(int index=0;index<256;index++) histogram[index]=0.0f; createHistogram(fileName, histogram, valueMaximum, valueMinimum); havePoints=true; std::cout<<" done."<<std::endl; break; default: std::cerr<<"Input file "<<argv[i]<<" has an unrecognized file format"<<std::endl; } } } /* Check if an output file name was given: */ if(outputFileName==0) { std::cerr<<"Usage: "<<argv[0]<<"-o <output file name> -np <max points per node> <input file spec>"<<std::endl; std::cerr<<"Input file spec: <format spec> <file name>"<<std::endl; std::cerr<<"Format spec: -XYZI"<<std::endl; std::cerr<<" -XYZRGB"<<std::endl; return 1; } /* Finish reading points: */ pa.finishReading(); loadTimer.elapse(); /* Construct an octree with less than maxPointsPerNode points per leaf: */ Misc::Timer createTimer; OctreeCreator tree(pa.getMaxNumCacheablePoints(),maxNumPointsPerNode,pa.getTempOctrees(),tempPointFileNameTemplate+"XXXXXX", histogram, valueMaximum, valueMinimum); /* Delete the temporary point octrees: */ pa.deleteTempOctrees(); createTimer.elapse(); /* Write the octree structure and data to the destination file: */ Misc::Timer writeTimer; tree.write(outputFileName); writeTimer.elapse(); std::cout<<"Time to load input data: "<<loadTimer.getTime()<<"s, time to create octree: "<<createTimer.getTime()<<"s, time to write final octree files: "<<writeTimer.getTime()<<"s"<<std::endl; return 0; }
int main(int argc,char* argv[]) { /* Open the depth stream file: */ IO::FilePtr depthFrameFile(IO::openFile("/work/okreylos/3DVideo/Kinect/DepthFrames.dat")); depthFrameFile->setEndianness(Misc::LittleEndian); /* Read the file header: */ unsigned int size[2]; depthFrameFile->read(size,2); /* Create a background frame: */ unsigned short* backgroundFrame=new unsigned short[size[1]*size[0]]; unsigned short* bfPtr=backgroundFrame; for(unsigned int y=0;y<size[1];++y) for(unsigned int x=0;x<size[0];++x,++bfPtr) *bfPtr=0x07ffU; unsigned int numCaptureFrames=150; /* Create the depth frame writer: */ IO::FilePtr compressedDepthFrameFile(IO::openFile("/work/okreylos/3DVideo/Kinect/CompressedDepthFrames.dat",IO::File::WriteOnly)); Kinect::DepthFrameWriter depthFrameWriter(*compressedDepthFrameFile,size); /* Process all frames from the depth frame file: */ size_t totalSize=0; double totalTime=0.0; double maxTime=0.0; unsigned int numFrames=0; while(!depthFrameFile->eof()) { /* Read the next frame's time stamp: */ double timeStamp=depthFrameFile->read<double>(); /* Read the next depth frame: */ Kinect::FrameBuffer frame(size[0],size[1],size[1]*size[0]*sizeof(unsigned short)); unsigned short* frameBuffer=static_cast<unsigned short*>(frame.getBuffer()); depthFrameFile->read(frameBuffer,size[1]*size[0]); if(numCaptureFrames>0) { /* Add the depth frame to the background frame: */ unsigned short* bfPtr=backgroundFrame; const unsigned short* dfPtr=frameBuffer; for(unsigned int y=0;y<size[1];++y) for(unsigned int x=0;x<size[0];++x,++bfPtr,++dfPtr) if(*bfPtr>*dfPtr-2) *bfPtr=*dfPtr-2; --numCaptureFrames; } else { /* Remove background from the depth frame: */ const unsigned short* bfPtr=backgroundFrame; unsigned short* dfPtr=frameBuffer; for(unsigned int y=0;y<size[1];++y) for(unsigned int x=0;x<size[0];++x,++bfPtr,++dfPtr) if(*dfPtr>=*bfPtr) *dfPtr=0x07ffU; } /* Compress and save the depth frame: */ Misc::Timer compressTime; compressedDepthFrameFile->write<double>(timeStamp); size_t compressedSize=depthFrameWriter.writeFrame(frame); compressTime.elapse(); double time=compressTime.getTime(); totalSize+=compressedSize; totalTime+=time; if(maxTime<time) maxTime=time; ++numFrames; } std::cout<<"Total compression time: "<<totalTime*1000.0<<" ms, total file size: "<<totalSize<<", "<<numFrames<<" frames"<<std::endl; std::cout<<"Maximum compression time: "<<maxTime*1000.0<<" ms"<<std::endl; std::cout<<"Compression frame rate: "<<double(numFrames)/totalTime<<" Hz"<<std::endl; std::cout<<"Bandwidth: "<<double(totalSize)*30.0/double(numFrames)/(1024.0*1024.0)<<"MB/s"<<std::endl; return 0; }
int main(int argc,char* argv[]) { /* Open the uncompressed depth stream file: */ IO::FilePtr depthFrameFile(IO::openFile("/work/okreylos/3DVideo/Kinect/DepthFrames.dat")); depthFrameFile->setEndianness(Misc::LittleEndian); /* Read the file header: */ unsigned int size[2]; depthFrameFile->read(size,2); /* Create a background frame: */ unsigned short* backgroundFrame=new unsigned short[size[1]*size[0]]; unsigned short* bfPtr=backgroundFrame; for(unsigned int y=0;y<size[1];++y) for(unsigned int x=0;x<size[0];++x,++bfPtr) *bfPtr=0x07ffU; unsigned int numCaptureFrames=150; /* Create the depth frame writer and reader: */ IO::FilePtr compressedDepthFrameFile(IO::openFile("/work/okreylos/3DVideo/Kinect/CompressedDepthFrames.dat",IO::File::ReadWrite)); Kinect::DepthFrameWriter depthFrameWriter(*compressedDepthFrameFile,size); compressedDepthFrameFile->flush(); Kinect::DepthFrameReader depthFrameReader(*compressedDepthFrameFile); /* Process all frames from the two depth frame files: */ double totalTime=0.0; double maxTime=0.0; unsigned int numFrames=0; while(!depthFrameFile->eof()) { /* Read the next uncompressed depth frame: */ Kinect::FrameBuffer frame0(size[0],size[1],size[1]*size[0]*sizeof(unsigned short)); frame0.timeStamp=depthFrameFile->read<double>(); unsigned short* frameBuffer0=frame0.getData<unsigned short>(); depthFrameFile->read(frameBuffer0,size[1]*size[0]); if(numCaptureFrames>0) { /* Add the depth frame to the background frame: */ unsigned short* bfPtr=backgroundFrame; const unsigned short* dfPtr=frameBuffer0; for(unsigned int y=0;y<size[1];++y) for(unsigned int x=0;x<size[0];++x,++bfPtr,++dfPtr) if(*bfPtr>*dfPtr-2) *bfPtr=*dfPtr-2; --numCaptureFrames; } else { /* Remove background from the depth frame: */ const unsigned short* bfPtr=backgroundFrame; unsigned short* dfPtr=frameBuffer0; for(unsigned int y=0;y<size[1];++y) for(unsigned int x=0;x<size[0];++x,++bfPtr,++dfPtr) if(*dfPtr>=*bfPtr) *dfPtr=0x07ffU; } /* Write the compressed depth frame: */ depthFrameWriter.writeFrame(frame0); compressedDepthFrameFile->flush(); /* Read the next compressed depth frame: */ Misc::Timer uncompressTime; Kinect::FrameBuffer frame1=depthFrameReader.readNextFrame(); uncompressTime.elapse(); double time=uncompressTime.getTime(); totalTime+=time; if(maxTime<time) maxTime=time; ++numFrames; /* Compare the two frames: */ unsigned int numPixels=size[0]*size[1]; const unsigned short* f0Ptr=frameBuffer0; const unsigned short* f1Ptr=frame1.getData<unsigned short>(); while(numPixels>0) { if(*f0Ptr!=*f1Ptr) std::cerr<<"Difference in frame "<<numFrames-1<<", "<<numPixels<<" pixels left"<<std::endl; ++f0Ptr; ++f1Ptr; --numPixels; } } std::cout<<"Total decompression time: "<<totalTime*1000.0<<" ms, "<<numFrames<<" frames"<<std::endl; std::cout<<"Maximum decompression time: "<<maxTime*1000.0<<" ms"<<std::endl; std::cout<<"Decompression frame rate: "<<double(numFrames)/totalTime<<" Hz"<<std::endl; return 0; }
void SpaceBall::deviceThreadMethod(void) { /* Create free-running timer to estimate tracker velocities: */ Misc::Timer timer; bool notFirstMeasurement=false; /* Receive lines from the serial port until interrupted: */ while(true) { /* Read characters until an end-of-line is encountered: */ unsigned char packet[256]; readPacket(256,packet); /* Determine the packet type: */ switch(packet[0]) { case 'D': { Vrui::VRDeviceState::TrackerState ts; /* Parse a data packet: */ short int rawData[6]; rawData[0]=(short int)(((unsigned int)packet[ 3]<<8)|(unsigned int)packet[ 4]); rawData[1]=(short int)(((unsigned int)packet[ 5]<<8)|(unsigned int)packet[ 6]); rawData[2]=(short int)(((unsigned int)packet[ 7]<<8)|(unsigned int)packet[ 8]); rawData[3]=(short int)(((unsigned int)packet[ 9]<<8)|(unsigned int)packet[10]); rawData[4]=(short int)(((unsigned int)packet[11]<<8)|(unsigned int)packet[12]); rawData[5]=(short int)(((unsigned int)packet[13]<<8)|(unsigned int)packet[14]); /* Calibrate linear data: */ PositionOrientation::Vector translation; for(int i=0;i<3;++i) translation[i]=double(rawData[i])*linearGain; translation[2]=-translation[2]; // Z-axis values are negated (?!?) /* Calibrate angular data: */ PositionOrientation::Vector rotationAxis; for(int i=0;i<3;++i) rotationAxis[i]=double(rawData[i+3])*angularGain; rotationAxis[2]=-rotationAxis[2]; // Z-axis values are negated (?!?) /* Construct incremental transformation: */ PositionOrientation t=PositionOrientation::translate(translation); PositionOrientation::Scalar rotationAngle=Geometry::mag(rotationAxis); PositionOrientation::Rotation rotation=PositionOrientation::Rotation::rotateAxis(rotationAxis,rotationAngle); t*=PositionOrientation::rotate(rotation); /* Accumulate current device position/orientation: */ currentPositionOrientation*=t; // currentPositionOrientation.leftMultiply(t); ts.positionOrientation=currentPositionOrientation; /* Calculate linear and angular velocities: */ timer.elapse(); if(notFirstMeasurement) { /* Estimate velocities by dividing position/orientation differences by elapsed time since last measurement: */ double time=timer.getTime(); ts.linearVelocity=translation/Vrui::VRDeviceState::TrackerState::LinearVelocity::Scalar(time); ts.angularVelocity=rotationAxis/Vrui::VRDeviceState::TrackerState::AngularVelocity::Scalar(time); } else { /* Force initial velocities to zero: */ ts.linearVelocity=Vrui::VRDeviceState::TrackerState::LinearVelocity::zero; ts.angularVelocity=Vrui::VRDeviceState::TrackerState::AngularVelocity::zero; notFirstMeasurement=true; } /* Update tracker state: */ setTrackerState(0,ts); break; } case '.': { /* Parse a button event packet: */ int buttonMask=0x0; buttonMask|=int(packet[2]&0x3f); buttonMask|=int(packet[2]&0x80)>>1; buttonMask|=int(packet[1]&0x1f)<<7; /* Update the current button states: */ for(int i=0;i<12;++i) setButtonState(i,buttonMask&(1<<i)); break; } } } }