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