[Jderobot-admin] jderobot-r1085 - in trunk/src/stable: components/openniServer libs/geometry/progeo
frivas en jderobot.org
frivas en jderobot.org
Jue Oct 31 15:46:25 CET 2013
Author: frivas
Date: 2013-10-31 15:46:25 +0100 (Thu, 31 Oct 2013)
New Revision: 1085
Modified:
trunk/src/stable/components/openniServer/CMakeLists.txt
trunk/src/stable/components/openniServer/myprogeo.cpp
trunk/src/stable/components/openniServer/myprogeo.h
trunk/src/stable/components/openniServer/openniServer.cfg
trunk/src/stable/components/openniServer/openniServer.cpp
trunk/src/stable/libs/geometry/progeo/Progeo.h
Log:
#91 included rgbCalibration files support
Modified: trunk/src/stable/components/openniServer/CMakeLists.txt
===================================================================
--- trunk/src/stable/components/openniServer/CMakeLists.txt 2013-10-31 14:15:23 UTC (rev 1084)
+++ trunk/src/stable/components/openniServer/CMakeLists.txt 2013-10-31 14:46:25 UTC (rev 1085)
@@ -50,7 +50,7 @@
${OpenCVGUI_LIBRARIES}
${gsl_LIBRARIES}
jderobotutil
- progeo
+ geometry
colorspacesmm
JderobotInterfaces
${OPENNI2_LIB}/libOpenNI2.so
@@ -66,7 +66,7 @@
${OpenCVGUI_LIBRARIES}
${gsl_LIBRARIES}
jderobotutil
- progeo
+ geometry
colorspacesmm
JderobotInterfaces
${OPENNI2_LIB}/libOpenNI2.so
Modified: trunk/src/stable/components/openniServer/myprogeo.cpp
===================================================================
--- trunk/src/stable/components/openniServer/myprogeo.cpp 2013-10-31 14:15:23 UTC (rev 1084)
+++ trunk/src/stable/components/openniServer/myprogeo.cpp 2013-10-31 14:46:25 UTC (rev 1085)
@@ -24,8 +24,12 @@
namespace openniServer {
-myprogeo::myprogeo() {
- std::cout << "CREADO" << std::endl;
+myprogeo::myprogeo(int nCam, int w, int h) {
+ for (int i=0; i< nCam; i++){
+ cameras[i]=new Progeo::Progeo();
+ }
+ this->w=w;
+ this->h=h;
}
myprogeo::~myprogeo() {
@@ -33,160 +37,81 @@
/* gets the calibration of the camera from a file */
-int myprogeo::load_cam_line(FILE *myfile,int cam)
+void myprogeo::load_cam(char *fich_in,int cam, int w, int h, bool fileFromCalibrator)
{
- char word1[MAX_BUFFER],word2[MAX_BUFFER];
- int i=0;
- char buffer_file[MAX_BUFFER];
- double roll;
+ std::cout << "Loading camera: " << cam << std::endl;
- 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);
- }
+ if (fileFromCalibrator){
+ std::cout << "File loaded as extra calibration " << std::endl;
+ this->cameras[cam]->readFromFile(std::string(fich_in));
- //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);
- }
+ this->RT2=this->cameras[cam]->getRTMatrix();
+ Eigen::Vector4d pos;
+ pos(0)=0;
+ pos(1)=0;
+ pos(2)=0;
+ pos(3)=1;
+ this->cameras[cam]->setPosition (pos);
- if (i >= MAX_BUFFER-1) {
- printf("%s...\n", buffer_file);
- printf ("Line too long in config file!\n");
- return -1;
+ Eigen::Vector4d foa;
+ foa(0)=0;
+ foa(1)=1;
+ foa(2)=0;
+ foa(3)=1;
+ this->cameras[cam]->setFoa(foa);
+ this->cameras[cam]->setRoll(0);
+ this->cameras[cam]->updateRTMatrix();
+
}
- buffer_file[++i]='\0';
+ else{
+ if (strlen(fich_in) ==0 ) {
+ std::cout << "Setting standard calibration" << std::endl;
+ Eigen::Matrix3d K;
+ K(0,0) = 511;
+ K(0,1) = 0;
+ K(0,2) = w/2;
- 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);
- }
- }
- return 1;
-}
+ K(1,0) = 0;
+ K(1,1) = 511;
+ K(1,2) = h/2;
-/* 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].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]);
+ K(2,0) = 0;
+ K(2,1) = 0;
+ K(2,2) = 1;
+ this->cameras[cam]->setKMatrix(K);
+ Eigen::Vector4d pos;
+ pos(0)=0;
+ pos(1)=0;
+ pos(2)=0;
+ pos(3)=1;
+ this->cameras[cam]->setPosition (pos);
+
+ Eigen::Vector4d foa;
+ foa(0)=0;
+ foa(1)=1;
+ foa(2)=0;
+ foa(3)=1;
+ this->cameras[cam]->setFoa(foa);
+ this->cameras[cam]->setRoll(0);
+
+ //this->cameras[cam]->updateKMatrix();
+ this->cameras[cam]->updateRTMatrix();
+
+
+ }
+ else {
+ std::cout << "Loading standard calibration file " << std::endl;
+ this->cameras[cam]->readFromFile(std::string(fich_in));
+ this->cameras[cam]->updateKMatrix();
+ this->cameras[cam]->updateRTMatrix();
+ }
}
- 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{
- do{i=load_cam_line(entrada,cam);}while(i!=EOF);
- fclose(entrada);
- } */
- display_camerainfo(cameras[cam]);
+ this->cameras[cam]->displayCameraInfo();
}
void myprogeo::pixel2optical(float*x,float*y){
@@ -208,65 +133,97 @@
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){
+ Eigen::Vector3d p;
+ Eigen::Vector4d pro;
+ pixel2optical(&x, &y);
+ p(0)=x;
+ p(1)=y;
+ p(2)=1;
- 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;
+ this->cameras[cam]->backproject(p,pro);
+ *xp=pro(0);
+ *yp=pro(1);
+ *zp=pro(2);
- *camx=cameras[cam].position.X;
- *camy=cameras[cam].position.Y;
- *camz=cameras[cam].position.Z;
+
+
+ Eigen::Vector4d pos;
+
+ pos=this->cameras[cam]->getPosition();
+
+ *camx=pos(0);
+ *camy=pos(1);
+ *camz=pos(2);
}
void
-myprogeo::myproject(float x, float y, float z, float* xp, float* yp, int cam) {
- HPoint2D p;
- HPoint3D p3;
+myprogeo::myproject(float x, float y, float z, float* xp, float* yp, int cam){
+ Eigen::Vector3d p;
+ Eigen::Vector4d p3;
- p3.X=x;
- p3.Y=y;
- p3.Z=z;
- p3.H=1;
+ p3(0)=x;
+ p3(1)=y;
+ p3(2)=z;
+ p3(3)=1;
- project(p3, &p, cameras[cam]);
- *xp=p.x;
- *yp=p.y;
+ this->cameras[cam]->project(p3,p);
+ *xp=p(0);
+ *yp=p(1);
}
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){
+ Eigen::Vector4d pos;
+
+ pos=this->cameras[cam]->getPosition();
+
+ *x=pos(0);
+ *y=pos(1);
+ *z=pos(2);
}
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;
+myprogeo::mygetcamerafoa(float *x, float *y, float *z, int cam){
+ Eigen::Vector4d foa;
+
+ foa=this->cameras[cam]->getFoa();
+ *x=foa(0);
+ *y=foa(1);
+ *z=foa(2);
}
void
-myprogeo::mygetcamerasize(float *w, float *h, int cam) {
- *w = cameras[cam].columns;
- *h = cameras[cam].rows;
+myprogeo::mygetcamerasize(float *w, float *h, int cam){
+ *w = this->w;
+ *h = this->h;
}
-TPinHoleCamera
-myprogeo::getCamera(int camera) {
- return cameras[camera];
+void
+myprogeo::applyExtraCalibration(float *x, float *y, float *z){
+ Eigen::Vector4d pos;
+
+ pos(0)=*x;
+ pos(1)=*y;
+ pos(2)=*z;
+ pos(3)=1;
+
+ //std::cout << "pre: " << pos << std::endl;
+
+ pos=this->RT2*pos;
+ //std::cout << "post: " << pos << std::endl;
+
+ *x=pos(0);
+ *y=pos(1);
+ *z=pos(2);
}
+Progeo::Progeo*
+myprogeo::getCamera(int camera){
+ return cameras[camera];
+}
+
} //namespace
Modified: trunk/src/stable/components/openniServer/myprogeo.h
===================================================================
--- trunk/src/stable/components/openniServer/myprogeo.h 2013-10-31 14:15:23 UTC (rev 1084)
+++ trunk/src/stable/components/openniServer/myprogeo.h 2013-10-31 14:46:25 UTC (rev 1085)
@@ -23,7 +23,7 @@
#ifndef OPENNISERVER_MYPROGEO_H
#define OPENNISERVER_MYPROGEO_H
-#include <progeo/progeo.h>
+#include <geometry/progeo/Progeo.h>
#include <iostream>
#include <string.h>
#include <stdio.h>
@@ -36,23 +36,26 @@
namespace openniServer {
class myprogeo {
public:
- myprogeo();
+ myprogeo(int nCam, int w, int h);
~myprogeo();
- int load_cam_line(FILE *myfile,int cam);
- void load_cam(char *fich_in,int cam, int w, int h);
+ void load_cam(char *fich_in,int cam, int w, int h, bool fileFromCalibrator);
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);
+ Progeo::Progeo* getCamera(int camera);
+ void new_camera();
+ void applyExtraCalibration(float *x, float *y, float *z);
+
private:
/* cameras */
void pixel2optical(float*x,float*y);
void optical2pixel(float* x, float* y);
- TPinHoleCamera cameras[MAX_CAMERAS];
+ Progeo::Progeo* cameras[MAX_CAMERAS];
int w,h;
+ Eigen::Matrix4d RT2; //transformada para la configuración del calibrador
};
} // namespace
Modified: trunk/src/stable/components/openniServer/openniServer.cfg
===================================================================
--- trunk/src/stable/components/openniServer/openniServer.cfg 2013-10-31 14:15:23 UTC (rev 1084)
+++ trunk/src/stable/components/openniServer/openniServer.cfg 2013-10-31 14:46:25 UTC (rev 1085)
@@ -4,7 +4,7 @@
#cameras configuration
openniServer.NCameras=1
openniServer.PlayerDetection=0
-openniServer.Mode=0
+openniServer.Mode=4
openniServer.ImageRegistration=1
@@ -22,7 +22,7 @@
openniServer.CameraRGB.PlayerDetection=0
openniServer.CameraRGB.Mirror=0
-#openniServer.calibration=./config/camRGB
+#openniServer.calibration=camera-0.cfg
openniServer.CameraDEPTH.Name=cameraB
openniServer.CameraDEPTH.Format=RGB8
openniServer.CameraDEPTH.fps=10
@@ -43,4 +43,5 @@
openniServer.pointCloud.Fps=10
openniServer.Pose3DMotorsActive=0
openniServer.KinectLedsActive=0
+openniServer.ExtraCalibration=0
Modified: trunk/src/stable/components/openniServer/openniServer.cpp
===================================================================
--- trunk/src/stable/components/openniServer/openniServer.cpp 2013-10-31 14:15:23 UTC (rev 1084)
+++ trunk/src/stable/components/openniServer/openniServer.cpp 2013-10-31 14:46:25 UTC (rev 1085)
@@ -940,11 +940,13 @@
int playerdetection = prop->getPropertyAsIntWithDefault("openniServer.PlayerDetection",0);
int fps =prop->getPropertyAsIntWithDefault("openniServer.pointCloud.Fps",10);
+ bool extra =(bool)prop->getPropertyAsIntWithDefault("openniServer.ExtraCalibration",0);
+ std::cout << "EXTRA: " << extra << std::endl;
#ifndef WITH_NITE2
playerdetection=0;
#endif
pthread_mutex_init(&this->localMutex, NULL);
- replyCloud = new ReplyCloud(this,prop->getProperty("openniServer.calibration"), playerdetection, depthVideoMode.getResolutionX(), depthVideoMode.getResolutionY(),fps);
+ replyCloud = new ReplyCloud(this,prop->getProperty("openniServer.calibration"), playerdetection, depthVideoMode.getResolutionX(), depthVideoMode.getResolutionY(),fps, extra);
this->control=replyCloud->start();
}
@@ -963,7 +965,7 @@
private:
class ReplyCloud :public IceUtil::Thread{
public:
- ReplyCloud (pointCloudI* pcloud, std::string filepath, int playerDetection, int widthIn, int heightIn, int fpsIn) : data(new jderobot::pointCloudData()), data2(new jderobot::pointCloudData()), _done(false)
+ ReplyCloud (pointCloudI* pcloud, std::string filepath, int playerDetection, int widthIn, int heightIn, int fpsIn, bool extra) : data(new jderobot::pointCloudData()), data2(new jderobot::pointCloudData()), _done(false)
{
path=filepath;
segmentation=playerDetection;
@@ -972,13 +974,13 @@
fps=fpsIn;
myCloud=pcloud;
mypro=NULL;
-
+ withExtraCalibration=extra;
}
void run()
{
- mypro= new openniServer::myprogeo();
- mypro->load_cam((char*)path.c_str(),0, cWidth, cHeight);
+ mypro= new openniServer::myprogeo(1,cWidth,cHeight);
+ mypro->load_cam((char*)path.c_str(),0, cWidth, cHeight,withExtraCalibration );
@@ -1039,6 +1041,12 @@
auxP.x=t*ux + camx;
auxP.y=t*uy+ camy;
auxP.z=t*uz + camz;
+
+
+ if (withExtraCalibration){
+ mypro->applyExtraCalibration(&auxP.x, &auxP.y, &auxP.z);
+ }
+
if ( segmentation){
auxP.id=pixelsID[i];
}
@@ -1093,6 +1101,7 @@
int segmentation;
pointCloudI* myCloud;
bool _done;
+ bool withExtraCalibration;
};
Modified: trunk/src/stable/libs/geometry/progeo/Progeo.h
===================================================================
--- trunk/src/stable/libs/geometry/progeo/Progeo.h 2013-10-31 14:15:23 UTC (rev 1084)
+++ trunk/src/stable/libs/geometry/progeo/Progeo.h 2013-10-31 14:46:25 UTC (rev 1085)
@@ -70,6 +70,8 @@
void setFoa(Eigen::Vector4d Foa);
void setRoll(float Roll);
+ Eigen::Matrix3d getKMatrix(){return K;};
+ Eigen::Matrix4d getRTMatrix(){return RT;};
Eigen::Vector4d getPosition() {return position; };
Eigen::Vector4d getFoa() {return foa; };
float getRoll() {return roll; };
More information about the Jderobot-admin
mailing list