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

194 lines
5.2 KiB
Text

/*
* PMD implementation
*
* Copyright (C) Stanislav Serebryakov
*
* Released under the GPL version 3.
*
*/
#include <cv.h> /* IplImage, cvCreateImage */
#include "pmdsdk2.h"
#include <stdlib.h>
#include <stdio.h>
#include "cvpmd.h"
#include <inttypes.h>
/** TODO
* inten max amplitude
* error handling
*/
PMD *initPMD(const char* plugin, const char *ip) {
PMD *io = (PMD*)malloc(sizeof(PMD));
if(0 != pmdOpen (&io->hnd, plugin, ip, plugin, "")) {
fprintf(stderr, "ERROR: could not connect!\n");
exit(1);
}
//pmdUpdate(io->hnd);
//pmdGetSourceDataDescription(io->hnd, &io->dd);
io->dd.std.numColumns = 64;
io->dd.std.numRows = 50; //FIXME!
io->data = (float*)malloc(io->dd.std.numColumns * io->dd.std.numRows * sizeof(float));
return io;
}
float *pmdDataPtr(PMD *p) {
return p->data;
}
void releasePMD(PMD **pmd) {
pmdClose((*pmd)->hnd);
free((*pmd)->data);
free(*pmd);
*pmd = 0;
}
static float max(const PMD *p) {
float max = 0.0f;
for(unsigned int k = 0; k < p->dd.std.numRows*p->dd.std.numColumns; k++) {
if(p->data[k] > max) max = p->data[k];
}
//printf("max = %f\n", max);
return max;
}
//static inline void setPix(const IplImage *m, const int c, const int r, const char v) { m->imageData[r*m->width + c] = v; }
CvSize pmdGetSize(const PMD *p) {
return cvSize(p->dd.std.numColumns, p->dd.std.numRows);
}
IplImage *toIplImage(const PMD *p, IplImage *img = 0) {
int cols = p->dd.std.numColumns;
int rows = p->dd.std.numRows;
IplImage *imgp;
if(!img) imgp = cvCreateImage(pmdGetSize(p), 8, 1);
else imgp = img;
//FIXME: scaled!
float m = max(p);
for(int r = 0; r < rows; r++) {
for(int c = 0; c < cols; c++) {
//FIXME: mess with the rows and colums
CV_IMAGE_ELEM(imgp, uint8_t, r, c) = (uint8_t) 255.0f * p->data[r*cols + c] / m;
}
}
return imgp;
}
/*CvPoint3D32f unionVec(const CvPoint uv, const CvMat *intrinsic) {
//TODO: not defined yet
// with this function pmdProjectToCartesian would look like:
// pmdProjectToCartesian pt depth mat = depth * unionVec pt mat
return cvPoint3D32f(0,0,0);
}*/
static inline CvPoint3D32f pmdProjectToCartesian(const CvMat *intrinsicMatrix, const CvPoint2D32f uv, const float dist) {
/* Lukas Dierks and Jan Wuelfing's projectToCartesian */
float fx = cvmGet(intrinsicMatrix, 0, 0);
float cx = cvmGet(intrinsicMatrix, 0, 2);
float fy = cvmGet(intrinsicMatrix, 1, 1);
float cy = cvmGet(intrinsicMatrix, 1, 2);
float u = uv.x;
float v = uv.y;
float r = dist;
float u2 = u*u;
float v2 = v*v;
float cx2 = cx*cx;
float cy2 = cy*cy;
float fx2 = fx*fx;
float fy2 = fy*fy;
// Reverse Projection
float squareroot = sqrt(
cy2 * fx2 +
cx2 * fy2 +
fx2 * fy2 -
2 * cx * fy2 * u +
fy2 * u2 -
2 * cy * fx2 * v +
fx2 * v2
);
return cvPoint3D32f((fy * r * (cx - u)) / squareroot,
(fx * r * (cy - v)) / squareroot,
(fx * fy * r) / squareroot);
}
CvPoint3D32f *cvProjectArrayToCartesian( const CvMat *intrinsicMatrix
, const CvPoint2D32f *pts, const int ptsCnt
, CvPoint3D32f *unitVs) {
for(int i = 0; i < ptsCnt; i++)
unitVs[i] = pmdProjectToCartesian(intrinsicMatrix, pts[i], 1.0);
return unitVs;
}
CvPoint3D32f **pmdProjectArrayToCartesian(const PMD *p, const CvMat *intrinsicMatrix, CvPoint3D32f **pts) {
for(unsigned int i = 0; i < p->dd.std.numRows; i++)
for(unsigned int j = 0; j < p->dd.std.numColumns; j++)
pts[i][j] = pmdProjectToCartesian(intrinsicMatrix, cvPoint2D32f(i,j), p->data[j*p->dd.std.numColumns + i]);
return pts;
}
IplImage *pmdQueryImage(PMD *p, IplImage *img = 0) {
pmdGetIntensities(p->hnd, p->data, p->dd.std.numColumns * p->dd.std.numRows * sizeof(float));
return toIplImage(p, img);
}
IplImage *pmdQueryImageAsync(PMD *p, IplImage *img = 0) {
pmdGetIntensitiesAsync(p->hnd, p->data, p->dd.std.numColumns * p->dd.std.numRows * sizeof(float));
return toIplImage(p, img);
}
void pmdRetriveDistances(PMD *p) {
pmdGetDistances(p->hnd, p->data, p->dd.std.numColumns * p->dd.std.numRows * sizeof(float));
return;
}
void pmdRetriveDistancesAsync(PMD *p) {
pmdGetDistancesAsync(p->hnd, p->data, p->dd.std.numColumns * p->dd.std.numRows * sizeof(float));
return;
}
IplImage *pmdQueryDistances(PMD *p, IplImage *img = 0) {
pmdGetDistances(p->hnd, p->data, p->dd.std.numColumns * p->dd.std.numRows * sizeof(float));
return toIplImage(p, img);
}
IplImage *pmdQueryDistancesAsync(PMD *p, IplImage *img = 0) {
pmdGetDistancesAsync(p->hnd, p->data, p->dd.std.numColumns * p->dd.std.numRows * sizeof(float));
return toIplImage(p, img);
}
IplImage *pmdQueryAmplitudes(PMD *p, IplImage *img = 0) {
pmdGetAmplitudesAsync(p->hnd, p->data, p->dd.std.numColumns * p->dd.std.numRows * sizeof(float));
return toIplImage(p, img);
}
IplImage *pmdQueryAmplitudesAsync(PMD *p, IplImage *img = 0) {
pmdGetAmplitudes(p->hnd, p->data, p->dd.std.numColumns * p->dd.std.numRows * sizeof(float));
return toIplImage(p, img);
}