[Jderobot-admin] jderobot-r1054 - trunk/src/stable/components/openni1Server
frivas en jderobot.org
frivas en jderobot.org
Mar Oct 15 12:21:38 CEST 2013
Author: frivas
Date: 2013-10-15 12:21:38 +0200 (Tue, 15 Oct 2013)
New Revision: 1054
Modified:
trunk/src/stable/components/openni1Server/myprogeo.cpp
trunk/src/stable/components/openni1Server/myprogeo.h
Log:
#66 solved problems with resolutions
Modified: trunk/src/stable/components/openni1Server/myprogeo.cpp
===================================================================
--- trunk/src/stable/components/openni1Server/myprogeo.cpp 2013-10-15 08:51:04 UTC (rev 1053)
+++ trunk/src/stable/components/openni1Server/myprogeo.cpp 2013-10-15 10:21:38 UTC (rev 1054)
@@ -24,220 +24,251 @@
namespace openni1Server {
-myprogeo::myprogeo(){
- std::cout << "CREADO" << std::endl;
+myprogeo::myprogeo() {
+ std::cout << "CREADO" << std::endl;
}
-myprogeo::~myprogeo(){
+myprogeo::~myprogeo() {
}
/* gets the calibration of the camera from a file */
int myprogeo::load_cam_line(FILE *myfile,int cam)
-{
- char word1[MAX_BUFFER],word2[MAX_BUFFER];
- int i=0;
- char buffer_file[MAX_BUFFER];
- double roll;
+{
+ char word1[MAX_BUFFER],word2[MAX_BUFFER];
+ int i=0;
+ char buffer_file[MAX_BUFFER];
+ double roll;
- buffer_file[0]=fgetc(myfile);
- if (feof(myfile)) return EOF;
- if (buffer_file[0]==(char)255) return EOF;
- if (buffer_file[0]=='#') {while(fgetc(myfile)!='\n'); return 0;}
- if (buffer_file[0]==' ') {while(buffer_file[0]==' ') buffer_file[0]=fgetc(myfile);}
- if (buffer_file[0]=='\t') {while(buffer_file[0]=='\t') buffer_file[0]=fgetc(myfile);}
-
- //Captures a line and then we will process it with sscanf checking that the last character is \n. We can't doit with fscanf because this function does not difference \n from blank space.
- while((buffer_file[i]!='\n') &&
- (buffer_file[i] != (char)255) &&
- (i<MAX_BUFFER-1) ) {
- buffer_file[++i]=fgetc(myfile);
- }
-
- if (i >= MAX_BUFFER-1) {
- printf("%s...\n", buffer_file);
- printf ("Line too long in config file!\n");
- return -1;
- }
- buffer_file[++i]='\0';
-
-
- if (sscanf(buffer_file,"%s",word1)!=1) return 0;
- // return EOF; empty line
- else {
- if (strcmp(word1,"positionX")==0){
- sscanf(buffer_file,"%s %s",word1,word2);
- cameras[cam].position.X=(float)atof(word2);
+ buffer_file[0]=fgetc(myfile);
+ if (feof(myfile)) return EOF;
+ if (buffer_file[0]==(char)255) return EOF;
+ if (buffer_file[0]=='#') {
+ while(fgetc(myfile)!='\n');
+ return 0;
}
- else if (strcmp(word1,"positionY")==0){
- sscanf(buffer_file,"%s %s",word1,word2);
- cameras[cam].position.Y=(float)atof(word2);
+ if (buffer_file[0]==' ') {
+ while(buffer_file[0]==' ') buffer_file[0]=fgetc(myfile);
}
- else if (strcmp(word1,"positionZ")==0){
- sscanf(buffer_file,"%s %s",word1,word2);
- cameras[cam].position.Z=(float)atof(word2);
+ if (buffer_file[0]=='\t') {
+ while(buffer_file[0]=='\t') buffer_file[0]=fgetc(myfile);
}
- else if (strcmp(word1,"positionH")==0){
- sscanf(buffer_file,"%s %s",word1,word2);
- cameras[cam].position.H=(float)atof(word2);
+
+ //Captures a line and then we will process it with sscanf checking that the last character is \n. We can't doit with fscanf because this function does not difference \n from blank space.
+ while((buffer_file[i]!='\n') &&
+ (buffer_file[i] != (char)255) &&
+ (i<MAX_BUFFER-1) ) {
+ buffer_file[++i]=fgetc(myfile);
}
- else if (strcmp(word1,"FOApositionX")==0){
- sscanf(buffer_file,"%s %s",word1,word2);
- cameras[cam].foa.X=(float)atof(word2);
+
+ if (i >= MAX_BUFFER-1) {
+ printf("%s...\n", buffer_file);
+ printf ("Line too long in config file!\n");
+ return -1;
}
- else if (strcmp(word1,"FOApositionY")==0){
- sscanf(buffer_file,"%s %s",word1,word2);
- cameras[cam].foa.Y=(float)atof(word2);
+ buffer_file[++i]='\0';
+
+
+ if (sscanf(buffer_file,"%s",word1)!=1) return 0;
+ // return EOF; empty line
+ else {
+ if (strcmp(word1,"positionX")==0) {
+ sscanf(buffer_file,"%s %s",word1,word2);
+ cameras[cam].position.X=(float)atof(word2);
+ }
+ else if (strcmp(word1,"positionY")==0) {
+ sscanf(buffer_file,"%s %s",word1,word2);
+ cameras[cam].position.Y=(float)atof(word2);
+ }
+ else if (strcmp(word1,"positionZ")==0) {
+ sscanf(buffer_file,"%s %s",word1,word2);
+ cameras[cam].position.Z=(float)atof(word2);
+ }
+ else if (strcmp(word1,"positionH")==0) {
+ sscanf(buffer_file,"%s %s",word1,word2);
+ cameras[cam].position.H=(float)atof(word2);
+ }
+ else if (strcmp(word1,"FOApositionX")==0) {
+ sscanf(buffer_file,"%s %s",word1,word2);
+ cameras[cam].foa.X=(float)atof(word2);
+ }
+ else if (strcmp(word1,"FOApositionY")==0) {
+ sscanf(buffer_file,"%s %s",word1,word2);
+ cameras[cam].foa.Y=(float)atof(word2);
+ }
+ else if (strcmp(word1,"FOApositionZ")==0) {
+ sscanf(buffer_file,"%s %s",word1,word2);
+ cameras[cam].foa.Z=(float)atof(word2);
+ }
+ else if (strcmp(word1,"FOApositionH")==0) {
+ sscanf(buffer_file,"%s %s",word1,word2);
+ cameras[cam].foa.H=(float)atof(word2);
+ }
+ else if (strcmp(word1,"roll")==0) {
+ sscanf(buffer_file,"%s %s",word1,word2);
+ cameras[cam].roll=(float)atof(word2);
+ }
+ else if (strcmp(word1,"f")==0) {
+ sscanf(buffer_file,"%s %s",word1,word2);
+ cameras[cam].fdistx=(float)atof(word2);
+ cameras[cam].fdisty=(float)atof(word2);
+ }
+ else if (strcmp(word1,"fx")==0) {
+ sscanf(buffer_file,"%s %s",word1,word2);
+ cameras[cam].fdistx=(float)atof(word2);
+ }
+ else if (strcmp(word1,"fy")==0) {
+ sscanf(buffer_file,"%s %s",word1,word2);
+ cameras[cam].fdisty=(float)atof(word2);
+ }
+ else if (strcmp(word1,"skew")==0) {
+ sscanf(buffer_file,"%s %s",word1,word2);
+ cameras[cam].skew=(float)atof(word2);
+ }
+ else if (strcmp(word1,"u0")==0) {
+ sscanf(buffer_file,"%s %s",word1,word2);
+ cameras[cam].u0=(float)atof(word2);
+ }
+ else if (strcmp(word1,"v0")==0) {
+ sscanf(buffer_file,"%s %s",word1,word2);
+ cameras[cam].v0=(float)atof(word2);
+ }
+ else if (strcmp(word1,"columns")==0) {
+ sscanf(buffer_file,"%s %s",word1,word2);
+ cameras[cam].columns=(int)atoi(word2);
+ }
+ else if (strcmp(word1,"rows")==0) {
+ sscanf(buffer_file,"%s %s",word1,word2);
+ cameras[cam].rows=(int)atoi(word2);
+ }
}
- else if (strcmp(word1,"FOApositionZ")==0){
- sscanf(buffer_file,"%s %s",word1,word2);
- cameras[cam].foa.Z=(float)atof(word2);
- }
- else if (strcmp(word1,"FOApositionH")==0){
- sscanf(buffer_file,"%s %s",word1,word2);
- cameras[cam].foa.H=(float)atof(word2);
- }
- else if (strcmp(word1,"roll")==0){
- sscanf(buffer_file,"%s %s",word1,word2);
- cameras[cam].roll=(float)atof(word2);
- }
- else if (strcmp(word1,"f")==0){
- sscanf(buffer_file,"%s %s",word1,word2);
- cameras[cam].fdistx=(float)atof(word2);
- cameras[cam].fdisty=(float)atof(word2);
- }
- else if (strcmp(word1,"fx")==0){
- sscanf(buffer_file,"%s %s",word1,word2);
- cameras[cam].fdistx=(float)atof(word2);
- }
- else if (strcmp(word1,"fy")==0){
- sscanf(buffer_file,"%s %s",word1,word2);
- cameras[cam].fdisty=(float)atof(word2);
- }
- else if (strcmp(word1,"skew")==0){
- sscanf(buffer_file,"%s %s",word1,word2);
- cameras[cam].skew=(float)atof(word2);
- }
- else if (strcmp(word1,"u0")==0){
- sscanf(buffer_file,"%s %s",word1,word2);
- cameras[cam].u0=(float)atof(word2);
- }
- else if (strcmp(word1,"v0")==0){
- sscanf(buffer_file,"%s %s",word1,word2);
- cameras[cam].v0=(float)atof(word2);
- }
- else if (strcmp(word1,"columns")==0){
- sscanf(buffer_file,"%s %s",word1,word2);
- cameras[cam].columns=(int)atoi(word2);
- }
- else if (strcmp(word1,"rows")==0){
- sscanf(buffer_file,"%s %s",word1,word2);
- cameras[cam].rows=(int)atoi(word2);
- }
- }
- return 1;
+ return 1;
}
/* gets the calibration of the camera from a file */
void myprogeo::load_cam(char *fich_in,int cam, int w, int h)
{
- FILE *entrada;
- int i;
- if (strlen(fich_in) ==0 ){
- std::cout << w << ", " << h << std::endl;
- this->cameras[cam].fdistx=515;
- this->cameras[cam].fdisty=515;
- this->cameras[cam].u0=h/2;
- this->cameras[cam].v0=w/2;
- this->cameras[cam].position.X=0;
- this->cameras[cam].position.Y=0;
- this->cameras[cam].position.Z=0;
- this->cameras[cam].foa.X=0;
- this->cameras[cam].foa.Y=1;
- this->cameras[cam].foa.Z=0;
- update_camera_matrix(&cameras[cam]);
+ this->w=w;
+ this->h=h;
+ FILE *entrada;
+ int i;
+ if (strlen(fich_in) ==0 ) {
+ std::cout << w << ", " << h << std::endl;
+ this->cameras[cam].fdistx=515;
+ this->cameras[cam].fdisty=515;
+ this->cameras[cam].v0=h/2;
+ this->cameras[cam].u0=w/2;
+ this->cameras[cam].position.X=0;
+ this->cameras[cam].position.Y=0;
+ this->cameras[cam].position.Z=0;
+ this->cameras[cam].foa.X=0;
+ this->cameras[cam].foa.Y=1;
+ this->cameras[cam].foa.Z=0;
+ this->cameras[cam].columns=w;
+ this->cameras[cam].rows=h;
+ this->w=w;
+ this->h=h;
+ update_camera_matrix(&cameras[cam]);
-
- }
- else{
- xmlReader(&(this->cameras[cam]), fich_in);
- update_camera_matrix(&cameras[cam]);
- }
- /*this->cameras[cam].position.H=1;
- this->cameras[cam].foa.H=1;*/
- /*std::cout << fich_in << std::endl;
- entrada=fopen(fich_in,"r");
- if(entrada==NULL){
+ }
+ else {
+ xmlReader(&(this->cameras[cam]), fich_in);
+ update_camera_matrix(&cameras[cam]);
+ }
+ /*this->cameras[cam].position.H=1;
+ this->cameras[cam].foa.H=1;*/
+
+ /*std::cout << fich_in << std::endl;
+ entrada=fopen(fich_in,"r");
+ if(entrada==NULL){
printf("tracker3D: camera input calibration file %s does not exits\n",fich_in);
- }else{
+ }else{
do{i=load_cam_line(entrada,cam);}while(i!=EOF);
fclose(entrada);
- } */
-
- display_camerainfo(cameras[cam]);
+ } */
+
+ display_camerainfo(cameras[cam]);
}
+void myprogeo::pixel2optical(float*x,float*y){
+ int localX=*x;
+ int localY=*y;
+ *x= this->h-1-localY;
+ *y= localX;
+}
+
+void myprogeo::optical2pixel(float*x,float*y){
+
+ int localX=*x;
+ int localY=*y;
+
+ *x= localY;
+ *y= this->h-1-localX;
+}
+
+
void
-myprogeo::mybackproject(float x, float y, float* xp, float* yp, float* zp, float* camx, float* camy, float* camz, int cam){
- HPoint2D p;
- HPoint3D pro;
-
+myprogeo::mybackproject(float x, float y, float* xp, float* yp, float* zp, float* camx, float* camy, float* camz, int cam) {
+ HPoint2D p;
+ HPoint3D pro;
- p.x=GRAPHIC_TO_OPTICAL_X(x,y);
- p.y=GRAPHIC_TO_OPTICAL_Y(x,y);
- p.h=1;
- backproject(&pro,p,cameras[cam]);
- *xp=pro.X;
- *yp=pro.Y;
- *zp=pro.Z;
- *camx=cameras[cam].position.X;
- *camy=cameras[cam].position.Y;
- *camz=cameras[cam].position.Z;
+ pixel2optical(&x, &y);
+ p.x=x;
+ p.y=y;
+ p.h=1;
+ backproject(&pro,p,cameras[cam]);
+ *xp=pro.X;
+ *yp=pro.Y;
+ *zp=pro.Z;
+
+ *camx=cameras[cam].position.X;
+ *camy=cameras[cam].position.Y;
+ *camz=cameras[cam].position.Z;
}
-void
-myprogeo::myproject(float x, float y, float z, float* xp, float* yp, int cam){
- HPoint2D p;
- HPoint3D p3;
+void
+myprogeo::myproject(float x, float y, float z, float* xp, float* yp, int cam) {
+ HPoint2D p;
+ HPoint3D p3;
- p3.X=x;
- p3.Y=y;
- p3.Z=z;
- p3.H=1;
-
- project(p3, &p, cameras[cam]);
- *xp=p.x;
- *yp=p.y;
+ p3.X=x;
+ p3.Y=y;
+ p3.Z=z;
+ p3.H=1;
+
+ project(p3, &p, cameras[cam]);
+ *xp=p.x;
+ *yp=p.y;
}
void
-myprogeo::mygetcameraposition(float *x, float *y, float *z, int cam){
- *x=cameras[cam].position.X;
- *y=cameras[cam].position.Y;
- *z=cameras[cam].position.Z;
+myprogeo::mygetcameraposition(float *x, float *y, float *z, int cam) {
+ *x=cameras[cam].position.X;
+ *y=cameras[cam].position.Y;
+ *z=cameras[cam].position.Z;
}
-void
-myprogeo::mygetcamerafoa(float *x, float *y, float *z, int cam){
- *x=cameras[cam].foa.X;
- *y=cameras[cam].foa.Y;
- *z=cameras[cam].foa.Z;
+void
+myprogeo::mygetcamerafoa(float *x, float *y, float *z, int cam) {
+ *x=cameras[cam].foa.X;
+ *y=cameras[cam].foa.Y;
+ *z=cameras[cam].foa.Z;
}
-void
-myprogeo::mygetcamerasize(float *w, float *h, int cam){
- *w = cameras[cam].columns;
- *h = cameras[cam].rows;
+void
+myprogeo::mygetcamerasize(float *w, float *h, int cam) {
+ *w = cameras[cam].columns;
+ *h = cameras[cam].rows;
}
-TPinHoleCamera
-myprogeo::getCamera(int camera){
- return cameras[camera];
+TPinHoleCamera
+myprogeo::getCamera(int camera) {
+ return cameras[camera];
}
} //namespace
Modified: trunk/src/stable/components/openni1Server/myprogeo.h
===================================================================
--- trunk/src/stable/components/openni1Server/myprogeo.h 2013-10-15 08:51:04 UTC (rev 1053)
+++ trunk/src/stable/components/openni1Server/myprogeo.h 2013-10-15 10:21:38 UTC (rev 1054)
@@ -20,8 +20,8 @@
*
*/
-#ifndef openni1Server_MYPROGEO_H
-#define openni1Server_MYPROGEO_H
+#ifndef OPENNISERVER_MYPROGEO_H
+#define OPENNISERVER_MYPROGEO_H
#include <progeo/progeo.h>
#include <iostream>
@@ -29,34 +29,31 @@
#include <stdio.h>
#include <stdlib.h>
-/* GRAPHIC coordenates to OPTICAL coordenates */
-#define WORKING_IMG_HEIGHT 480
-#define GRAPHIC_TO_OPTICAL_X(x,y) (WORKING_IMG_HEIGHT-1-y)
-#define GRAPHIC_TO_OPTICAL_Y(x,y) (x)
-#define OPTICAL_TO_GRAPHIC_X(x,y) (y)
-#define OPTICAL_TO_GRAPHIC_Y(x,y) (WORKING_IMG_HEIGHT-1-x)
#define MAX_CAMERAS 8
#define MAX_BUFFER 1024
namespace openni1Server {
- class myprogeo {
- public:
- myprogeo();
- ~myprogeo();
- int load_cam_line(FILE *myfile,int cam);
- void load_cam(char *fich_in,int cam, int w, int h);
- void mybackproject(float x, float y, float* xp, float* yp, float* zp, float* camx, float* camy, float* camz, int cam);
- void myproject(float x, float y, float z, float* xp, float* yp, int cam);
- void mygetcameraposition(float *x, float *y, float *z, int cam);
- void mygetcamerafoa(float *x, float *y, float *z, int cam);
- void mygetcamerasize(float *w, float *h, int cam);
- TPinHoleCamera getCamera(int camera);
+class myprogeo {
+public:
+ myprogeo();
+ ~myprogeo();
+ int load_cam_line(FILE *myfile,int cam);
+ void load_cam(char *fich_in,int cam, int w, int h);
+ void mybackproject(float x, float y, float* xp, float* yp, float* zp, float* camx, float* camy, float* camz, int cam);
+ void myproject(float x, float y, float z, float* xp, float* yp, int cam);
+ void mygetcameraposition(float *x, float *y, float *z, int cam);
+ void mygetcamerafoa(float *x, float *y, float *z, int cam);
+ void mygetcamerasize(float *w, float *h, int cam);
+ TPinHoleCamera getCamera(int camera);
- private:
- /* cameras */
- TPinHoleCamera cameras[MAX_CAMERAS];
- };
+private:
+ /* cameras */
+ void pixel2optical(float*x,float*y);
+ void optical2pixel(float* x, float* y);
+ TPinHoleCamera cameras[MAX_CAMERAS];
+ int w,h;
+};
} // namespace
#endif /*KINECTVIEWER_MYPROGEO_H*/
More information about the Jderobot-admin
mailing list