60 lines
1.5 KiB
Text
60 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;
|
||
|
|
}
|
||
|
|
|
||
|
|
|