3dpcp/.svn/pristine/7e/7e1d7fcb819b726036d5dc6402297f79535b2f13.svn-base
2012-09-16 14:33:11 +02:00

59 lines
1.5 KiB
Text

/*
* converToSLAM6D implementation
*
* Copyright (C) Stanislav Serebryakov
*
* Released under the GPL version 3.
*
*/
#include <pmdWrap.h>
int main() {
PMDCam *pmdc = initPMDCam("./pmdc.conf");
int frames = 0;
unsigned int latestTime = 0;
while(1) {
if(grabData(pmdc)) break;
if(latestTime >= pmdc->timestamp) continue;
latestTime = pmdc->timestamp;
//printf("%i %i\n", latestTime, pmdc->timestamp);
CvSize pmdSz = cvGetSize(pmdc->iPMDI);
pmdSz.width = 50;
char filename[] = "./dat/scan000.pose";
sprintf(filename, "./dat/scan%03d.3d", frames);
FILE *scan = fopen(filename, "wb");
if(!scan) fprintf(stderr, "ERROR: cant create file %s.\n", filename);
sprintf(filename, "./dat/scan%03d.pose", frames);
FILE *pose = fopen(filename, "wb");
if(!pose) fprintf(stderr, "ERROR: cant create file %s.\n", filename);
fprintf(scan, "%i x %i\n", pmdSz.width, pmdSz.height);
for(int j = 0; j < pmdSz.width; j++)
for(int i = 0; i < pmdSz.height; i++)
fprintf(scan, "%f %f %f\n", pmdc->pts[i][j].x*100.0, pmdc->pts[i][j].y*100.0, pmdc->pts[i][j].z*100.0);
fprintf(pose, "%f %f %f\n%f %f %f\n", 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
if(0 == frames%100) printf("+100\n");
fflush(scan);
fflush(pose);
fclose(scan);
fclose(pose);
frames++;
}
return 0;
}