3dpcp/.svn/pristine/04/04c746477138e4481e4395485fdcee196c90315a.svn-base
2012-09-16 14:33:11 +02:00

122 lines
3.9 KiB
Text

/*
* grabVideoAnd3D implementation
*
* Copyright (C) Stanislav Serebryakov
*
* Released under the GPL version 3.
*
*/
#include <stdio.h>
#include <stdlib.h>
#include <cv.h>
#include <highgui.h>
#include "pmdsdk2.h"
#include "cvpmd.h"
int main(int argc, char **argv)
{
int totalFrames = 0;
int ui = 1;
printf("%i\n", argc);
if(argc > 1) {
totalFrames = atoi(argv[1]);
ui = 0;
}
if(argc > 2) ui = 1;
//FIXME: here is lots of old code, should be removed
PMD *pmd = initPMD("../o3d.L32.pcp", "192.168.0.69");
CvCapture *capture = cvCaptureFromCAM(1); //FIXME: should be passed via argc
CvSize pmdSz = pmdGetSize(pmd);
printf("pmd sz: %i x %i\n", pmdSz.width, pmdSz.height);
IplImage *imgCamColor = cvQueryFrame(capture);
IplImage *imgPMD = cvCreateImage(pmdGetSize(pmd), 8, 1);
IplImage *imgPMDA = cvCreateImage(cvGetSize(imgPMD), 8, 1);
IplImage *imgCam = cvCreateImage(cvGetSize(imgCamColor), 8, 1);
CvPoint3D32f **pmdPts = (CvPoint3D32f**) cvAlloc(pmdSz.height * sizeof(CvPoint3D32f*));
for(int i = 0; i < pmdSz.height; i++) pmdPts[i] = (CvPoint3D32f*) cvAlloc(pmdSz.width * sizeof(CvPoint3D32f));
CvMat *intrinsicCam = (CvMat*)cvLoad("../intrinsic-cam-6x4.xml");
CvMat *distortionCam = (CvMat*)cvLoad("../distortion-cam-6x4.xml");
CvMat *intrinsicPMD = (CvMat*)cvLoad("../intrinsic-pmd-6x4.xml");
CvMat *distortionPMD = (CvMat*)cvLoad("../distortion-pmd-6x4.xml");
if(!intrinsicCam || !distortionCam|| !intrinsicPMD || !distortionPMD) {
fprintf(stderr, "ERROR: can't load intrinsic and/or distortion xml files!\n");
return 1;
}
// FILE *vPMDA = fopen("./s001.arv", "wb"); // ala dot Amplitudes Raw Video NOTE: i'm not sure it is amplitude :P
// FILE *vPMDAf = fopen("./s001.farv", "wb"); // same but Floating
FILE *vPMDI = fopen("./s001.irv", "wb"); // same but Intensities
FILE *vPMDIf = fopen("./s001.firv", "wb");
FILE *headers = fopen("./s001.head", "wb");
CvVideoWriter *vCam = cvCreateVideoWriter( "./s001.avi"
, CV_FOURCC('D', 'I', 'V', 'X')
, 25, cvGetSize(imgCam), 1);
FILE *pmdPtsFile = fopen("./s001.3dp", "w");
if(ui) {
cvNamedWindow("Cam", 0);
cvNamedWindow("PMD", 1);
}
printf("DEBUG: init done, going to grab %i frames.\n", totalFrames);
int frames = 0;
while(1) {
frames++;
if(0 == frames % 100) printf("%i frames grabbed...\n", frames);
// Image retriving
pmdUpdate(pmd->hnd);
imgCamColor = cvQueryFrame(capture);
pmdQueryImageAsync(pmd, imgPMD);
fwrite(pmdDataPtr(pmd), sizeof(float), pmdSz.width*pmdSz.height, vPMDIf);
//pmdQueryAmplitudesAsync(pmd, imgPMDA);
//fwrite(pmdDataPtr(pmd), sizeof(float), pmdSz.width*pmdSz.height, vPMDAf);
pmdRetriveDistancesAsync(pmd);
pmdProjectArrayToCartesian(pmd, intrinsicPMD, pmdPts);
ImageHeaderInformation *header = retriveHeader();
if(ui) {
cvShowImage("Cam", imgCamColor);
cvShowImage("PMD", imgPMD);
}
//FIXME: order col/str
for(int i = 0; i < pmdSz.height; i++)
fwrite(pmdPts[i], sizeof(CvPoint3D32f), pmdSz.width, pmdPtsFile);
fwrite(imgPMD->imageData, sizeof(char), pmdSz.width*pmdSz.height, vPMDI);
//fwrite(imgPMDA->imageData, sizeof(char), pmdSz.width*pmdSz.height, vPMDA);
fwrite(header, sizeof(ImageHeaderInformation), 1, headers);
cvWriteFrame(vCam, imgCamColor);
if(totalFrames && (frames >= totalFrames)) break;
if(ui) if(27 == cvWaitKey(5)) break;
}
// fclose(vPMDA);
// fclose(vPMDAf);
fclose(vPMDI);
fclose(vPMDIf);
fclose(pmdPtsFile);
fclose(headers);
printf("grabbed %i frames.\n", frames);
printf("See s001* files (you want to rename them).\n");
}