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

290 lines
11 KiB
Text

/*
* PMDCam implementation
*
* Copyright (C) Stanislav Serebryakov
*
* Released under the GPL version 3.
*
*/
#include <stdio.h>
#include <stdlib.h>
#include <cv.h>
#include <highgui.h>
#include <libconfig.h>
#include "cvpmd.h"
#include "pmdWrap.h"
#include "slam6d/icp6Dhelix.h"
#include "slam6d/icp6Dortho.h"
#include "slam6d/icp6Dquat.h"
#include "slam6d/icp6Dsvd.h"
#include "slam6d/icp6Dapx.h"
//TODO: releasePMDCam()
//TODO: !!! check config parse error !!! TODO
PMDCam *initPMDCam(const char *confPath) {
PMDCam *pmdc = (PMDCam*)malloc(sizeof(PMDCam));
config_t *conf = (config_t*)malloc(sizeof(config_t));
config_init(conf);
config_read_file(conf, confPath);
config_lookup_bool(conf, "offline", &pmdc->_offlineMode);
if(!pmdc->_offlineMode) {
// init PMDCam
const char *ip;
const char *plugin;
config_lookup_string(conf, "ip", &ip);
config_lookup_string(conf, "plugin", &plugin);
printf("Starting PMD...\n");
pmdc->_pmd = initPMD(plugin, ip);
//FIXME: pmdc initialization assert
int camID;
#if (((LIBCONFIG_VER_MAJOR == 1) && (LIBCONFIG_VER_MINOR >= 4)) \
|| (LIBCONFIG_VER_MAJOR > 1))
config_lookup_int(conf, "cameraID", &camID);
#else //libconfig API changed in version 1.4b
config_lookup_int(conf, "cameraID", (long *)&camID);
#endif
pmdc->_capture = cvCaptureFromCAM(camID);
if(!pmdc->_capture) fprintf(stderr, "ERROR: Can't initialize capture, see camera id in pmdc.conf.\n");
} else {
printf("Offline mode.\n");
const char *camVid;
const char *pmdI;
const char *pmdA;
const char *pmd3D;
const char *headers;
config_lookup_string(conf, "camVideoFile", &camVid);
config_lookup_string(conf, "pmdIFile", &pmdI);
// config_lookup_string(conf, "pmdAFile", &pmdA);
config_lookup_string(conf, "pmd3DFile", &pmd3D);
config_lookup_string(conf, "headersFile", &headers);
//FIXME: check empty strings
printf( "DEBUG: Reading from files: %s, %s, %s.\n"
, camVid, pmdI, pmd3D);
pmdc->_capture = cvCaptureFromFile(camVid);
pmdc->_f.i = fopen(pmdI, "r");
// pmdc->_f.a = fopen(pmdA, "r");
pmdc->_f.p = fopen(pmd3D, "r");
pmdc->_f.h = fopen(headers, "r");
if( !pmdc->_capture
|| !pmdc->_f.i
// || !pmdc->_f.a
|| !pmdc->_f.p
|| !pmdc->_f.h) fprintf(stderr, "ERROR: Can't open data file, see files settings in pmdc.conf.\n");
pmdc->header = (ImageHeaderInformation*)cvAlloc(sizeof(ImageHeaderInformation));
//TODO: read pmd header somewhere
}
CvSize pmdSz;
#if (((LIBCONFIG_VER_MAJOR == 1) && (LIBCONFIG_VER_MINOR >= 4)) \
|| (LIBCONFIG_VER_MAJOR > 1))
config_lookup_int(conf, "pmdSize.width", &pmdSz.width);
config_lookup_int(conf, "pmdSize.height", &pmdSz.height);
#else //libconfig API changed in version 1.4b
config_lookup_int(conf, "pmdSize.width", (long *)&pmdSz.width);
config_lookup_int(conf, "pmdSize.height", (long *)&pmdSz.height);
#endif
printf("DEBUG: pmdSz: %i %i\n", pmdSz.width, pmdSz.height);
pmdc->_iPMDIU = cvCreateImage(pmdSz, IPL_DEPTH_8U, 1);
pmdc->iPMDI = cvCreateImage(pmdSz, IPL_DEPTH_8U, 1);
pmdc->_iPMDAU = cvCreateImage(pmdSz, IPL_DEPTH_8U, 1);
pmdc->iPMDA = cvCreateImage(pmdSz, IPL_DEPTH_8U, 1);
pmdc->_iCamColorU = cvQueryFrame(pmdc->_capture);
CvSize camSz = cvGetSize(pmdc->_iCamColorU);
printf("DEBUG: camSz: %i %i\n", camSz.width, camSz.height);
config_lookup_bool(conf, "hybrid", &pmdc->hybrid);
if(pmdc->hybrid) pmdc->_iCamColorUBuffer = cvCreateImage(camSz, IPL_DEPTH_8U, 3);
pmdc->iCamColor = cvCreateImage(camSz, IPL_DEPTH_8U, 3);
pmdc->iCam = cvCreateImage(camSz, IPL_DEPTH_8U, 1);
pmdc->_mapXPMD = cvCreateImage(pmdSz, IPL_DEPTH_32F, 1);
pmdc->_mapYPMD = cvCreateImage(pmdSz, IPL_DEPTH_32F, 1);
pmdc->_mapXCam = cvCreateImage(camSz, IPL_DEPTH_32F, 1);
pmdc->_mapYCam = cvCreateImage(camSz, IPL_DEPTH_32F, 1);
printf("DEBUG: Created images...\n");
const char *inc;
const char *dsc;
const char *inp;
const char *dsp;
config_lookup_string(conf, "intrinsicCam", &inc);
config_lookup_string(conf, "distortionCam", &dsc);
config_lookup_string(conf, "intrinsicPMD", &inp);
config_lookup_string(conf, "distortionPMD", &dsp);
pmdc->intrinsicCam = (CvMat*)cvLoad(inc);
pmdc->distortionCam = (CvMat*)cvLoad(dsc);
pmdc->intrinsicPMD = (CvMat*)cvLoad(inp);
pmdc->distortionPMD = (CvMat*)cvLoad(dsp);
if(!pmdc->intrinsicCam || !pmdc->distortionCam || !pmdc->intrinsicPMD || !pmdc->distortionPMD)
fprintf(stderr, "ERROR: Cant load matrix file, see pmdc.conf matrix files settings.\n");
//TODO: essential mat
cvInitUndistortMap(pmdc->intrinsicCam, pmdc->distortionCam, pmdc->_mapXCam, pmdc->_mapYCam);
cvInitUndistortMap(pmdc->intrinsicPMD, pmdc->distortionPMD, pmdc->_mapXPMD, pmdc->_mapYPMD);
printf("DEBUG: initialized undistortion maps...\n");
CvPoint3D32f **pmdPts = (CvPoint3D32f**) cvAlloc(pmdSz.height * sizeof(CvPoint3D32f*));
for(int i = 0; i < pmdSz.height; i++) pmdPts[i] = (CvPoint3D32f*) cvAlloc(pmdSz.width * sizeof(CvPoint3D32f));
pmdc->pts = pmdPts;
config_lookup_float(conf, "tracking.quality", &pmdc->_track.quality);
#if (((LIBCONFIG_VER_MAJOR == 1) && (LIBCONFIG_VER_MINOR >= 4)) \
|| (LIBCONFIG_VER_MAJOR > 1))
config_lookup_int(conf, "tracking.min", &pmdc->_track.minFeatures);
config_lookup_int(conf, "tracking.max", &pmdc->_track.maxFeatures);
config_lookup_int(conf, "tracking.minDist", &pmdc->_track.minDist);
config_lookup_int(conf, "tracking.winSize", &pmdc->_track.winSz);
config_lookup_int(conf, "historyLen", &pmdc->historyLen);
#else //libconfig API changed in version 1.4b
config_lookup_int(conf, "tracking.min", (long *)&pmdc->_track.minFeatures);
config_lookup_int(conf, "tracking.max", (long *)&pmdc->_track.maxFeatures);
config_lookup_int(conf, "tracking.minDist", (long *)&pmdc->_track.minDist);
config_lookup_int(conf, "tracking.winSize", (long *)&pmdc->_track.winSz);
config_lookup_int(conf, "historyLen", (long *)&pmdc->historyLen);
#endif
pmdc->_track.trackingFlags = 0; //FIXME, if you init pyrs here
#if (((LIBCONFIG_VER_MAJOR == 1) && (LIBCONFIG_VER_MINOR >= 4)) \
|| (LIBCONFIG_VER_MAJOR > 1))
config_lookup_int(conf, "minPts", &pmdc->minPts4Pose);
#else //libconfig API changed in version 1.4b
config_lookup_int(conf, "minPts", (long *)&pmdc->minPts4Pose);
#endif
config_lookup_float(conf, "maxError", &pmdc->maxError);
config_lookup_float(conf, "outliersRemoval.sigmaDepth", &pmdc->sigmaDepth);
config_lookup_float(conf, "outliersRemoval.sigmaColor", &pmdc->sigmaColor);
config_lookup_float(conf, "outliersRemoval.threshold", &pmdc->dpThreshold);
config_lookup_bool(conf, "savePoses", &pmdc->savePoses);
config_lookup_bool(conf, "synchronous", &pmdc->synchronous);
int icpVerbose;
config_lookup_bool(conf, "icp.verbose", &icpVerbose);
bool icpQuiet = !icpVerbose;
const char *icpMethod;
config_lookup_string(conf, "icp.method", &icpMethod);
printf("%s\n", icpMethod);
if(!strcmp(icpMethod, "helix")) pmdc->icp = new icp6D_HELIX(icpQuiet);
else if(!strcmp(icpMethod, "svd")) pmdc->icp = new icp6D_SVD(icpQuiet);
else if(!strcmp(icpMethod, "apx")) pmdc->icp = new icp6D_APX(icpQuiet);
else if(!strcmp(icpMethod, "quat")) pmdc->icp = new icp6D_QUAT(icpQuiet);
else if(!strcmp(icpMethod, "ortho")) pmdc->icp = new icp6D_ORTHO(icpQuiet);
else {
fprintf(stderr, "ERROR: Uknown ICP method: %s!\n", icpMethod);
exit(1);
}
//TODO: init tracking?
//TODO: alloc pts
config_destroy(conf);
return pmdc;
}
int grabData(PMDCam *pmdc) {
static unsigned int latestTimestamp = 0;
static unsigned int latestTimestampUsec = 0;
while(1) { // grab data until the new have come
if(!pmdc->_offlineMode) {
pmdUpdate(pmdc->_pmd->hnd);
pmdc->_iCamColorUBuffer = cvQueryFrame(pmdc->_capture);
if(pmdc->synchronous) {
pmdRetriveDistances(pmdc->_pmd);
pmdProjectArrayToCartesian(pmdc->_pmd, pmdc->intrinsicPMD, pmdc->pts);
// project array has to be called right after retrive distances
pmdQueryImageAsync(pmdc->_pmd, pmdc->_iPMDIU);
} else {
pmdRetriveDistancesAsync(pmdc->_pmd);
pmdProjectArrayToCartesian(pmdc->_pmd, pmdc->intrinsicPMD, pmdc->pts);
// project array has to be called right after retrive distances
pmdQueryImageAsync(pmdc->_pmd, pmdc->_iPMDIU);
/* printf("time: %f, utime: %f, valid: %f\n"
, pmdc->header->Seconds
, pmdc->header->Useconds
, pmdc->header->ValidImage
);*/
}
pmdc->header = retriveHeader();
pmdc->timestamp = (unsigned int)pmdc->header->Seconds;
pmdc->timestampUsec = (unsigned int)pmdc->header->Useconds;
if(pmdc->hybrid) {
if( pmdc->timestamp > latestTimestamp
|| pmdc->timestampUsec > latestTimestampUsec) {
latestTimestamp = pmdc->timestamp;
latestTimestampUsec = pmdc->timestampUsec;
cvCopy(pmdc->_iCamColorUBuffer, pmdc->_iCamColorU);
break;
} else continue;
} else {
pmdc->_iCamColorU = pmdc->_iCamColorUBuffer;
break;
}
} else { // offliine mode
pmdc->_iCamColorUBuffer = cvQueryFrame(pmdc->_capture);
if(!pmdc->_iCamColorUBuffer) return 1;
CvSize pmdSz = cvGetSize(pmdc->_iPMDIU);
if(fread(pmdc->_iPMDIU->imageData, sizeof(char), pmdSz.width*pmdSz.height, pmdc->_f.i)
< (unsigned)pmdSz.width*pmdSz.height) return 1;
//if(fread(pmdc->_iPMDAU->imageData, sizeof(char), pmdSz.width*pmdSz.height, pmdc->_f.a)
// < (unsigned)pmdSz.width*pmdSz.height) return 1;
for(int i = 0; i < pmdSz.height; i++)
fread(pmdc->pts[i], sizeof(CvPoint3D32f), pmdSz.width, pmdc->_f.p);
fread(pmdc->header, sizeof(ImageHeaderInformation), 1, pmdc->_f.h);
// i hope CvPt32f contains no holes :)
pmdc->timestamp = (unsigned int)pmdc->header->Seconds;
pmdc->timestampUsec = (unsigned int)pmdc->header->Useconds;
/*printf("%i %i %i\n", (unsigned int)pmdc->header->Seconds
, (unsigned int)pmdc->header->Useconds
, (unsigned int)pmdc->header->ValidImage);*/
if(pmdc->hybrid) {
if( pmdc->timestamp > latestTimestamp
|| pmdc->timestampUsec > latestTimestampUsec) {
latestTimestamp = pmdc->timestamp;
latestTimestampUsec = pmdc->timestampUsec;
cvCopy(pmdc->_iCamColorUBuffer, pmdc->_iCamColorU);
break;
} else continue;
} else {
pmdc->_iCamColorU = pmdc->_iCamColorUBuffer;
break;
}
}
}
cvFlip(pmdc->_iPMDIU, 0, 1); // flip around x-axes
cvRemap(pmdc->_iPMDIU, pmdc->iPMDI, pmdc->_mapXPMD, pmdc->_mapYPMD); //undistortion
cvFlip(pmdc->_iCamColorU, 0, -1); // flip around x and y axes
cvRemap(pmdc->_iCamColorU, pmdc->iCamColor, pmdc->_mapXCam, pmdc->_mapYCam);
cvCvtColor(pmdc->iCamColor, pmdc->iCam, CV_BGR2GRAY);
return 0;
}