[Jderobot-admin] jderobot-r920 - in trunk/src/components/calibratorKinect: . build-independent

frivas en jderobot.org frivas en jderobot.org
Lun Mayo 27 18:45:09 CEST 2013


Author: frivas
Date: 2013-05-27 18:44:09 +0200 (Mon, 27 May 2013)
New Revision: 920

Modified:
   trunk/src/components/calibratorKinect/CMakeLists.txt
   trunk/src/components/calibratorKinect/build-independent/CMakeLists.txt
   trunk/src/components/calibratorKinect/calibratorKinect.cpp
   trunk/src/components/calibratorKinect/common.h
   trunk/src/components/calibratorKinect/controller.cpp
   trunk/src/components/calibratorKinect/controller.h
   trunk/src/components/calibratorKinect/drawarea.cpp
   trunk/src/components/calibratorKinect/drawarea.h
   trunk/src/components/calibratorKinect/view.cpp
Log:
nueva codificaion de imagen de profundidad en calibratorkinect


Modified: trunk/src/components/calibratorKinect/CMakeLists.txt
===================================================================
--- trunk/src/components/calibratorKinect/CMakeLists.txt	2013-05-27 15:20:58 UTC (rev 919)
+++ trunk/src/components/calibratorKinect/CMakeLists.txt	2013-05-27 16:44:09 UTC (rev 920)
@@ -28,5 +28,6 @@
  ${LIBS_DIR}/colorspaces/libcolorspacesmm.so
  ${INTERFACES_CPP_DIR}/jderobot/libJderobotInterfaces.so
  ${LIBS_DIR}/progeo/libprogeo.so
+ ${LIBS_DIR}/parallelIce/libparallelIce.so
     #-lxerces-c
 )

Modified: trunk/src/components/calibratorKinect/build-independent/CMakeLists.txt
===================================================================
--- trunk/src/components/calibratorKinect/build-independent/CMakeLists.txt	2013-05-27 15:20:58 UTC (rev 919)
+++ trunk/src/components/calibratorKinect/build-independent/CMakeLists.txt	2013-05-27 16:44:09 UTC (rev 920)
@@ -77,4 +77,5 @@
   ${LIBS_DIR}/libjderobotice.so
   ${LIBS_DIR}/libjderobotutil.so
   ${LIBS_DIR}/libprogeo.so
+    ${LIBS_DIR}/libparallelIce.so
 )

Modified: trunk/src/components/calibratorKinect/calibratorKinect.cpp
===================================================================
--- trunk/src/components/calibratorKinect/calibratorKinect.cpp	2013-05-27 15:20:58 UTC (rev 919)
+++ trunk/src/components/calibratorKinect/calibratorKinect.cpp	2013-05-27 16:44:09 UTC (rev 920)
@@ -1,6 +1,6 @@
 /*
 *
-*  Copyright (C) 1997-2010 JDERobot Developers Team
+*  Copyright (C) 1997-2013 JDERobot Developers Team
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
@@ -17,6 +17,7 @@
 *
 *  Authors : Sara Marugán Alonso <smarugan en gsyc.es>,
  *           Eduardo Perdices <eperdices en gsyc.es>
+ *           Francisco Miguel Rivas Montero <franciscomiguel.rivas en urjc.es>
 *
 */
 
@@ -57,23 +58,20 @@
 		for (int i=0; i< nCameras; i++){
 			std::ostringstream sTemp;
 			std::ostringstream sTemp2;
-			sTemp << "CalibratorKinect.CameraRGB." << i << ".Proxy";
-			Ice::ObjectPrx baseRGB = ic->propertyToProxy(sTemp.str());
-			sTemp2 << "CalibratorKinect.CameraDEPTH." << i << ".Proxy";
-			Ice::ObjectPrx baseDEPTH = ic->propertyToProxy(sTemp2.str());
-			if (0==baseRGB)
-				throw "Could not create proxy to RGB camera";
-			if (0==baseDEPTH)
-				throw "Could not create proxy to DEPTH camera";
+			sTemp << "CalibratorKinect.CameraRGB." << i << ".";
+			sources[i].RGB=new jderobot::cameraClient(ic, sTemp.str());
+			sTemp2 << "CalibratorKinect.CameraDEPTH." << i << ".";
+			sources[i].DEPTH=new jderobot::cameraClient(ic, sTemp2.str());
 
-			/*cast to CameraPrx*/
-			sources[i].cRGBprx = jderobot::CameraPrx::checkedCast(baseRGB);
-			if (0==sources[i].cRGBprx)
-				throw "Invalid RGB proxy";
 
-			sources[i].cDEPTHprx = jderobot::CameraPrx::checkedCast(baseDEPTH);
-			if (0==sources[i].cDEPTHprx)
-				throw "Invalid DEPTH proxy";
+			if (sources[i].RGB==NULL)
+				throw "Could not create proxy to RGB camera";
+			else
+				sources[i].RGB->start();
+			if (sources[i].DEPTH==NULL)
+				throw "Could not create proxy to DEPTH camera";
+			else
+				sources[i].DEPTH->start();
 		}
 
 
@@ -85,56 +83,11 @@
 
 
 		while(view->isVisible()){
-			/*Get image*/
-
-			/*jderobot::ImageDataPtr dataRGB = sources[cam].cRGBprx->getImageData();
-			colorspaces::Image::FormatPtr fmt = colorspaces::Image::Format::searchFormat(dataRGB->description->format);
-			if (!fmt)
-				throw "Format not supported";
-			jderobot::ImageDataPtr dataDEPTH = sources[cam].cDEPTHprx->getImageData();
-			colorspaces::Image::FormatPtr fmt2 = colorspaces::Image::Format::searchFormat(dataDEPTH->description->format);
-			if (!fmt2)
-				throw "Format not supported";
-
-			colorspaces::Image image(dataRGB->description->width, dataRGB->description->height, fmt, &(dataRGB->pixelData[0]));
-			colorspaces::Image imageDEPTH(dataDEPTH->description->width, dataDEPTH->description->height, fmt2, &(dataDEPTH->pixelData[0]));*/
-
 			view->display(sources);
-			usleep(10*1000);
+
+			usleep(1000);
 		}
 
-/*libcalibrator_init(worldconf.c_str(),camInconf.c_str(),camOutconf.c_str());
-libcalibrator_initgui();
-
-unsigned char image[IMAGE_WIDTH*IMAGE_HEIGHT*3];
-
-for(;;){
-jderobot::ImageDataPtr data = cprx->getImageData();
-colorspaces::Image::FormatPtr fmt = colorspaces::Image::Format::searchFormat(data->description->format);
-if (!fmt)
-throw "Format not supported";
-
-colorspaces::Image img(data->description->width,
-data->description->height,
-fmt,
-&(data->pixelData[0]));
-
-if(img.width!=IMAGE_WIDTH || img.height!=IMAGE_HEIGHT){
-throw "Image size not supported";
-}*/
-
-/* BGR to RGB */
-/* int size=img.width*img.height;
-for (int j=0; j<size; j++) {
-image[j*3] = img.data[j*3+2];
-image[j*3+1] = img.data[j*3+1];
-image[j*3+2] = img.data[j*3];
-}
-
-libcalibrator_guibuttons();
-libcalibrator_guidisplay((unsigned char*)image);
-}*/
-
 	}catch (const Ice::Exception& ex) {
 		std::cerr << ex << std::endl;
 		status = 1;

Modified: trunk/src/components/calibratorKinect/common.h
===================================================================
--- trunk/src/components/calibratorKinect/common.h	2013-05-27 15:20:58 UTC (rev 919)
+++ trunk/src/components/calibratorKinect/common.h	2013-05-27 16:44:09 UTC (rev 920)
@@ -19,15 +19,15 @@
  *             Jose María Cañas Plaza <jmplaza en gsyc.es>
  *
  */
-#include <jderobot/camera.h>
+#include <parallelIce/cameraClient.h>
 
 #ifndef calibrator_COMMON_H
 #define calibrator_COMMON_H
 namespace CalibratorKinect {
 
 struct kinectData{
-	jderobot::CameraPrx cRGBprx;
-	jderobot::CameraPrx cDEPTHprx;
+	jderobot::cameraClient* RGB;
+	jderobot::cameraClient* DEPTH;
 };
 
 

Modified: trunk/src/components/calibratorKinect/controller.cpp
===================================================================
--- trunk/src/components/calibratorKinect/controller.cpp	2013-05-27 15:20:58 UTC (rev 919)
+++ trunk/src/components/calibratorKinect/controller.cpp	2013-05-27 16:44:09 UTC (rev 920)
@@ -1,5 +1,5 @@
 /*
- *  Copyright (C) 1997-2010 JDERobot Developers Team
+*  Copyright (C) 1997-2013 JDERobot Developers Team
  *
  *  This program is free software; you can redistribute it and/or modify
  *  it under the terms of the GNU General Public License as published by
@@ -16,6 +16,7 @@
  *  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
  *
  *   Authors : Eduardo Perdices <eperdices en gsyc.es>,
+ *   			Francisco Miguel Rivas Montero <franciscomiguel.rivas en urjc.es>
  *             Jose María Cañas Plaza <jmplaza en gsyc.es>
  *
  */
@@ -181,8 +182,6 @@
 		std::string camera = prop->getProperty(strInd.str());
 		xmlReader(&(this->cameras[cam]), camera.c_str());
 
-		std::cout << camera << std::endl;
-		std::cout << this->cameras[cam].position.X << std::endl;
 
 		//old calibration
 	    /*Ice::PropertyDict pd = prop->getPropertiesForPrefix("CalibratorKinect.Config");
@@ -207,23 +206,21 @@
   }
 
   void
-  Controller::drawWorld(const colorspaces::Image& image, int cam) {
-    IplImage src;
-    CvPoint p, q;
+  Controller::drawWorld(cv::Mat image, int cam) {
+    cv::Point p, q;
     HPoint3D p1, p2;
     int n=0;
 
     /*Update camera with current values*/
     update_camera_matrix(&(this->cameras[cam]));
 
-    src=image;
 
     for(vector<HPoint3D>::iterator it = this->lines.begin(); it != this->lines.end(); it++) {
       if(n%2==0) {
 	p1 = (*it);
       } else {
 	p2 = (*it);
-	this->drawLine(&src, p1, p2, cam);
+	this->drawLine(image, p1, p2, cam);
       }
       n++;
     }
@@ -234,20 +231,20 @@
       p.y = 0;
       q.x = cWidth/2;
       q.y = cHeight-1;
-      cvLine(&src, p, q, CV_RGB(255,0,0), 1, CV_AA, 0);
+      cv::line(image, p, q, cv::Scalar(255,0,0), 1, CV_AA, 0);
       p.x = 0;
       p.y = cHeight/2;
       q.x = cWidth-1;
       q.y = cHeight/2;
-      cvLine(&src, p, q, CV_RGB(255,0,0), 1, CV_AA, 0);
+      cv::line(image, p, q, cv::Scalar(255,0,0), 1, CV_AA, 0);
     }
   }
 
   void
-  Controller::drawLine(IplImage * src, HPoint3D pini, HPoint3D pend, int cam) {
+  Controller::drawLine(cv::Mat  src, HPoint3D pini, HPoint3D pend, int cam) {
     HPoint2D p1, p2;
     HPoint2D gooda,goodb;
-    CvPoint pt1, pt2;
+    cv::Point pt1, pt2;
 
     project(pini,&p1,this->cameras[cam]);
     project(pend,&p2,this->cameras[cam]);
@@ -260,15 +257,13 @@
       pt2.x=(int)goodb.y;
       pt2.y=this->cameras[cam].rows-1-(int)goodb.x;
       
-      cvLine(src, pt1, pt2, cvScalar(255, 0, 255, 0), 0.5, 8, 0);
+      cv::line(src, pt1, pt2, cv::Scalar(255, 0, 255, 0), 0.5, 8, 0);
     }
   }
 
   int
   Controller::load_camera_config(Ice::PropertyDict pd, int cam) {
-	  std::cout << "cargo" << std::endl;
     for(Ice::PropertyDict::const_iterator it = pd.begin(); it != pd.end(); it++) {
-    	std::cout << (*it).first << std::endl;
 
 		std::istringstream sTemp;
       if((*it).first.compare("CalibratorKinect.Config.Position.X")==0) {
@@ -387,141 +382,102 @@
   }
 
 
-	void Controller::add_depth_pointsImage(const colorspaces::Image& imageDEPTH, const colorspaces::Image& imageRGB, CalibratorKinect::DrawArea* world, int cam, int scale, int colour){
-	float distance;
+	void Controller::add_depth_pointsImage(cv::Mat distances, cv::Mat imageRGB, CalibratorKinect::DrawArea* world, int cam, int scale, int colour){
+	float d;
 
-		
-		//std::cout << "inicio reconstrucción" << std::endl;
-		//std::cout << cWidth*cHeight << std::endl;
-		for( unsigned int i = 0 ; i < cWidth*cHeight ; i=i+scale) {
-			if (((int)imageDEPTH.data[3*i+0]==255) && ( ((int)imageDEPTH.data[3*i+1] >0 ) && ((int)imageDEPTH.data[3*i+1] < 255)) && (((int)imageDEPTH.data[3*i+2] > 0 ) && ((int)imageDEPTH.data[3*i+2] < 255))){
-					distance = -((int)imageDEPTH.data[3*i+1] - 255);
-				}
-				else if (((int)imageDEPTH.data[3*i+0]==255) && (((int)imageDEPTH.data[3*i+1] >= 0) && ((int)imageDEPTH.data[3*i+1]<255)) && ((int)imageDEPTH.data[3*i+2] == 0)){
-					distance = 255+ (int)imageDEPTH.data[3*i+1];
-				}
-				else if ((((int)imageDEPTH.data[3*i+0] >= 0 ) && ((int)imageDEPTH.data[3*i+0] < 255))  && ((int)imageDEPTH.data[3*i+1]==255) && ((int)imageDEPTH.data[3*i+2] == 0)){
-					distance = 2*255 - ((int)imageDEPTH.data[3*i+0] - 255);
-				}
-				else if (((int)imageDEPTH.data[3*i+0] == 0) && ((int)imageDEPTH.data[3*i+1]==255) && (((int)imageDEPTH.data[3*i+2] >= 0 ) && ((int)imageDEPTH.data[3*i+2] < 255))){
-					distance = 3*255 + (int)imageDEPTH.data[3*i+2];
-				}
-				else if (((int)imageDEPTH.data[3*i+0] == 0) && ((int)imageDEPTH.data[3*i+1] >= 0 ) && ((int)imageDEPTH.data[3*i+1] < 255) && ((int)imageDEPTH.data[3*i+2]==255)){
-					distance = 4*255 - ((int)imageDEPTH.data[3*i+1] - 255);
-				}
-				else if (((int)imageDEPTH.data[3*i+0] == 0) && ((int)imageDEPTH.data[3*i+1] == 0) && ((int)imageDEPTH.data[3*i+2] > 0 ) && ((int)imageDEPTH.data[3*i+2] < 255)){
-					distance = 5*255 - ((int)imageDEPTH.data[3*i+2] - 255);
-				}
-				else{
-					distance=0;
-				}
+		for (int xIm=0; xIm< cWidth; xIm++){
+			for (int yIm=0; yIm<cHeight ; yIm++){
+				d=distances.at<float>(yIm,xIm);
 
-				if (distance != 0 ){
-					//PRUEBA DE DISTANCIA		
-					/*if (i==153600 + 320){
-						std::cout << distance << std::endl;
-					}*/
 
-				distance = distance *10;
-			
-				float xp,yp,zp,camx,camy,camz;
-				float ux,uy,uz; 
-				float x,y;
-				float k;
-				float c1x, c1y, c1z;
-				float fx,fy,fz;
-				float fmod;
-				float t;
-				float Fx,Fy,Fz;
-			
-		
-				//mybackproject(float x, float y, float* xp, float* yp, float* zp, float* camx, float* camy, float* camz, int cam){
-				//mybackproject(i % cWidth, i / cWidth, &xp, &yp, &zp, &camx, &camy, &camz,0);
-				
-				/*mybackproyect*/
-				x=i % (int)cWidth;
-				y= i / (int) cWidth;
-				HPoint2D p;
-				HPoint3D pro;
-				p.x=GRAPHIC_TO_OPTICAL_X(x,y,cHeight); 
-				p.y=GRAPHIC_TO_OPTICAL_Y(x,y,cHeight);
-				p.h=1;
-				backproject(&pro,p,this->cameras[cam]);
-				xp=pro.X;
-				yp=pro.Y;
-				zp=pro.Z;
+				if (d != 0 ){
+					float xp,yp,zp,camx,camy,camz;
+					float ux,uy,uz;
+					float x,y;
+					float k;
+					float c1x, c1y, c1z;
+					float fx,fy,fz;
+					float fmod;
+					float t;
+					float Fx,Fy,Fz;
 
-				camx=this->cameras[cam].position.X;
-				camy=this->cameras[cam].position.Y;
-				camz=this->cameras[cam].position.Z;
-				/*end*/
+					HPoint2D p;
+					HPoint3D pro;
+					p.x=GRAPHIC_TO_OPTICAL_X(xIm,yIm,cHeight);
+					p.y=GRAPHIC_TO_OPTICAL_Y(xIm,yIm,cHeight);
+					p.h=1;
+					backproject(&pro,p,this->cameras[cam]);
+					xp=pro.X;
+					yp=pro.Y;
+					zp=pro.Z;
 
+					camx=this->cameras[cam].position.X;
+					camy=this->cameras[cam].position.Y;
+					camz=this->cameras[cam].position.Z;
+
 			
-				//vector unitario
-				float modulo;
+					//vector unitario
+					float modulo;
 			
-				modulo = sqrt(1/(((camx-xp)*(camx-xp))+((camy-yp)*(camy-yp))+((camz-zp)*(camz-zp))));
+					modulo = sqrt(1/(((camx-xp)*(camx-xp))+((camy-yp)*(camy-yp))+((camz-zp)*(camz-zp))));
 
-				//mypro->mygetcameras[cam]foa(&c1x, &c1y, &c1z, 0);
-				c1x=this->cameras[cam].foa.X;
-				c1y=this->cameras[cam].foa.Y;
-				c1z=this->cameras[cam].foa.Z;
+					//mypro->mygetcameras[cam]foa(&c1x, &c1y, &c1z, 0);
+					c1x=this->cameras[cam].foa.X;
+					c1y=this->cameras[cam].foa.Y;
+					c1z=this->cameras[cam].foa.Z;
 
-				fmod = sqrt(1/(((camx-c1x)*(camx-c1x))+((camy-c1y)*(camy-c1y))+((camz-c1z)*(camz-c1z))));
-				fx = (c1x - camx)*fmod;
-				fy = (c1y - camy)*fmod;
-				fz = (c1z - camz) * fmod;
-				ux = (xp-camx)*modulo;
-				uy = (yp-camy)*modulo;
-				uz = (zp-camz)*modulo;
+					fmod = sqrt(1/(((camx-c1x)*(camx-c1x))+((camy-c1y)*(camy-c1y))+((camz-c1z)*(camz-c1z))));
+					fx = (c1x - camx)*fmod;
+					fy = (c1y - camy)*fmod;
+					fz = (c1z - camz) * fmod;
+					ux = (xp-camx)*modulo;
+					uy = (yp-camy)*modulo;
+					uz = (zp-camz)*modulo;
 
-				Fx= distance*fx + camx;
-				Fy= distance*fy + camy;
-				Fz= distance*fz + camz;
+					Fx= d*fx + camx;
+					Fy= d*fy + camy;
+					Fz= d*fz + camz;
 
-				/* calculamos el punto real */
-				t = (-(fx*camx) + (fx*Fx) - (fy*camy) + (fy*Fy) - (fz*camz) + (fz*Fz))/((fx*ux) + (fy*uy) + (fz*uz));
+					/* calculamos el punto real */
+					t = (-(fx*camx) + (fx*Fx) - (fy*camy) + (fy*Fy) - (fz*camz) + (fz*Fz))/((fx*ux) + (fy*uy) + (fz*uz));
 
-			
 
-				/*world->points[i][0]=distance*ux+camx;
-				world->points[i][1]=distance*uy+camy;
-				world->points[i][2]=distance*uz+camz;*/
-				/*std::cout << xp << "," << yp << "," << zp << "," << modulo << std::endl;
-				std::cout << xp-camx << "," << yp-camy<< "," << zp-camz << std::endl;
-				std::cout << ux << "," << uy<< "," << uz << std::endl;*/
-				//k= (80-yp)/uy;
-				//std::cout << "distancia" << distance << std::endl;
-			
-				if (colour==0)
-					world->add_kinect_point(t*ux + camx,t*uy+ camy,t*uz + camz,(int)imageRGB.data[3*i],(int)imageRGB.data[3*i+1],(int)imageRGB.data[3*i+2]);
-				else{
-					int r,g,b;
-					if (colour==1){
-						r=255;
-						g=0;
-						b=0;
-					}
-					else if (colour==2){
-						r=0;
-						g=0;
-						b=255;
-					}
+
+					/*world->points[i][0]=distance*ux+camx;
+					world->points[i][1]=distance*uy+camy;
+					world->points[i][2]=distance*uz+camz;*/
+					/*std::cout << xp << "," << yp << "," << zp << "," << modulo << std::endl;
+					std::cout << xp-camx << "," << yp-camy<< "," << zp-camz << std::endl;
+					std::cout << ux << "," << uy<< "," << uz << std::endl;*/
+					//k= (80-yp)/uy;
+					//std::cout << "distancia" << distance << std::endl;
+
+					if (colour==0)
+						world->add_kinect_point(t*ux + camx,t*uy+ camy,t*uz + camz,(int)imageRGB.data[3*(yIm*cWidth+xIm)],(int)imageRGB.data[3*(yIm*cWidth+xIm)+1],(int)imageRGB.data[3*(yIm*cWidth+xIm)+2]);
 					else{
-						r=0;
-						g=1;
-						b=0;
+						int r,g,b;
+						if (colour==1){
+							r=255;
+							g=0;
+							b=0;
+						}
+						else if (colour==2){
+							r=0;
+							g=0;
+							b=255;
+						}
+						else{
+							r=0;
+							g=1;
+							b=0;
+						}
+						world->add_kinect_point(t*ux + camx,t*uy+ camy,t*uz + camz,r,g,b);
 					}
-					world->add_kinect_point(t*ux + camx,t*uy+ camy,t*uz + camz,r,g,b);
-				}
 
-
-
 				//world->add_line(distance*ux + camx,distance*uy+ camy,distance*uz + camz,camx,camy,camz);
 				}
+			}
 		}
-
-	//std::cout << "fin reconstrucción" << std::endl;
 }
 
 

Modified: trunk/src/components/calibratorKinect/controller.h
===================================================================
--- trunk/src/components/calibratorKinect/controller.h	2013-05-27 15:20:58 UTC (rev 919)
+++ trunk/src/components/calibratorKinect/controller.h	2013-05-27 16:44:09 UTC (rev 920)
@@ -1,5 +1,5 @@
 /*
-*  Copyright (C) 1997-2010 JDERobot Developers Team
+*  Copyright (C) 1997-2013 JDERobot Developers Team
  *
  *  This program is free software; you can redistribute it and/or modify
  *  it under the terms of the GNU General Public License as published by
@@ -16,6 +16,7 @@
  *  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
  *
  *   Authors : Eduardo Perdices <eperdices en gsyc.es>,
+ *   			Francisco Miguel Rivas Montero <franciscomiguel.rivas en urjc.es>
  *             Jose María Cañas Plaza <jmplaza en gsyc.es>
  *
  */
@@ -54,11 +55,11 @@
     
     std::string getGladePath();
 
-		void drawWorld(const colorspaces::Image& image,int cam);
+		void drawWorld(cv::Mat image,int cam);
 
-		//OJO asignar valores
-		float cWidth;
-		float cHeight;
+		//Tamaño de la imagen
+		int cWidth;
+		int cHeight;
 
 		HPoint3D * getPos(int cam);
 		HPoint3D * getFoa(int cam);
@@ -76,7 +77,7 @@
 		void setV0(float value,int cam);	
 		void setRoll(float value,int cam);
 		void changeDrawCenter();
-		void add_depth_pointsImage(const colorspaces::Image& imageDEPTH, const colorspaces::Image& imageRGB, CalibratorKinect::DrawArea* world,int cam, int scale, int colour);
+		void add_depth_pointsImage(cv::Mat distances, cv::Mat imageRGB, CalibratorKinect::DrawArea* world,int cam, int scale, int colour);
 		void saveCameras(int nCameras);
 
   private:
@@ -86,7 +87,7 @@
 		int load_camera_config(Ice::PropertyDict pd,int cam);
 		int load_world();
 		
-		void drawLine(IplImage * src, HPoint3D pini, HPoint3D pend, int cam);
+		void drawLine(cv::Mat , HPoint3D pini, HPoint3D pend, int cam);
 
 		std::string gladepath;
 		std::string world;

Modified: trunk/src/components/calibratorKinect/drawarea.cpp
===================================================================
--- trunk/src/components/calibratorKinect/drawarea.cpp	2013-05-27 15:20:58 UTC (rev 919)
+++ trunk/src/components/calibratorKinect/drawarea.cpp	2013-05-27 16:44:09 UTC (rev 920)
@@ -190,6 +190,9 @@
 	}
 
 	bool DrawArea::on_expose_event(GdkEventExpose* event) {
+	}
+
+	void DrawArea::my_expose_event(){
 		//std::cout << "expose" << std::endl;
 		Gtk::Allocation allocation = get_allocation();
 		GLfloat width, height;
@@ -233,8 +236,6 @@
 			glFlush();
 
 		glwindow->gl_end();
-
-		return true;
 	}
 
 	bool DrawArea::on_timeout() {

Modified: trunk/src/components/calibratorKinect/drawarea.h
===================================================================
--- trunk/src/components/calibratorKinect/drawarea.h	2013-05-27 15:20:58 UTC (rev 919)
+++ trunk/src/components/calibratorKinect/drawarea.h	2013-05-27 16:44:09 UTC (rev 920)
@@ -90,6 +90,7 @@
 		void setToUserCamera ();
 		void readFile(std::string path);
 		virtual bool on_expose_event(GdkEventExpose* event);
+		void my_expose_event();
 		void add_line(float x0,float y0, float z0, float x1, float y1, float z1);
 		void add_camera_line(float x0,float y0, float z0, float x1, float y1, float z1);
 		void add_kinect_point(float x,float y, float z,int r,int g, int b);

Modified: trunk/src/components/calibratorKinect/view.cpp
===================================================================
--- trunk/src/components/calibratorKinect/view.cpp	2013-05-27 15:20:58 UTC (rev 919)
+++ trunk/src/components/calibratorKinect/view.cpp	2013-05-27 16:44:09 UTC (rev 920)
@@ -75,7 +75,7 @@
 
         
 		/*Set default config*/
-		std::cout << (double)this->controller->getPos(cam)->X << std::endl;
+
 		//vscale_pos_x->set_value((double)this->controller->getPos(cam)->X);
 		vscale_pos_x->set_value(12);
 		vscale_pos_y->set_value((double)this->controller->getPos(cam)->Y);
@@ -129,8 +129,6 @@
   m_Combo->pack_start(m_Columns.m_col_name);
 
 
-	std::cout << "creado" << std::endl;
-
 	}
 
 	View::~View() {
@@ -147,105 +145,119 @@
 
 		/*Manage image*/
 		
+		/*Set image*/
 
-		/*Set image*/
+
 		if (w_depth->get_active()){
-			jderobot::ImageDataPtr dataDEPTH = sources[cam].cDEPTHprx->getImageData();
-			colorspaces::Image::FormatPtr fmt2 = colorspaces::Image::Format::searchFormat(dataDEPTH->description->format);
-			if (!fmt2)
-				throw "Format not supported";
-			colorspaces::Image imageDEPTH(dataDEPTH->description->width, dataDEPTH->description->height, fmt2, &(dataDEPTH->pixelData[0]));
 
-			this->controller->drawWorld(imageDEPTH, cam);
-			colorspaces::ImageRGB8 img_rgb8(imageDEPTH);//conversion will happen if needed
-			Glib::RefPtr<Gdk::Pixbuf> imgBuff = Gdk::Pixbuf::create_from_data((const guint8*)img_rgb8.data,
+
+			cv::Mat srcDEPTH=sources[cam].DEPTH->getImage();
+
+
+			/*split channels to separate distance from image*/
+			std::vector<cv::Mat> layers;
+			cv::split(srcDEPTH, layers);
+			cv::Mat colorDepth(srcDEPTH.size(),srcDEPTH.type());
+			cv::cvtColor(layers[0],colorDepth,CV_GRAY2RGB);
+
+			controller->drawWorld(colorDepth,cam);
+
+
+			Glib::RefPtr<Gdk::Pixbuf> imgBuff = Gdk::Pixbuf::create_from_data((const guint8*)colorDepth.data,
 				    Gdk::COLORSPACE_RGB,
 				    false,
 				    8,
-				    img_rgb8.width,
-				    img_rgb8.height,
-				    img_rgb8.step); 
+				    colorDepth.cols,
+				    colorDepth.rows,
+				    colorDepth.step);
 			gtk_image->clear();
     		gtk_image->set(imgBuff);
 			mainwindow->resize(1,1);
 			while (gtkmain.events_pending()){
       			gtkmain.iteration();
-	}
+			}
 		}
 		else{
-			jderobot::ImageDataPtr dataRGB = sources[cam].cRGBprx->getImageData();
-			colorspaces::Image::FormatPtr fmt = colorspaces::Image::Format::searchFormat(dataRGB->description->format);
-			if (!fmt)
-				throw "Format not supported";
-			colorspaces::Image image(dataRGB->description->width, dataRGB->description->height, fmt, &(dataRGB->pixelData[0]));
-			this->controller->drawWorld(image, cam);
-			colorspaces::ImageRGB8 img_rgb8(image);//conversion will happen if needed
-			Glib::RefPtr<Gdk::Pixbuf> imgBuff = Gdk::Pixbuf::create_from_data((const guint8*)img_rgb8.data,
+
+			cv::Mat rgb = sources[cam].RGB->getImage();
+
+			controller->drawWorld(rgb,cam);
+			Glib::RefPtr<Gdk::Pixbuf> imgBuff = Gdk::Pixbuf::create_from_data((const guint8*)rgb.data,
 				    Gdk::COLORSPACE_RGB,
 				    false,
 				    8,
-				    img_rgb8.width,
-				    img_rgb8.height,
-				    img_rgb8.step);
+				    rgb.cols,
+				    rgb.rows,
+				    rgb.step);
 			gtk_image->clear();
     		gtk_image->set(imgBuff);
 			mainwindow->resize(1,1);
+
+			//world->my_expose_event();
 			while (gtkmain.events_pending()){
       			gtkmain.iteration();
-	}
+
+
+			}
+
 		}
 
+
+
 		//creo el mundo en 3D.
 
 		if (w_fix->get_active()){
 			this->world->clear_points();
 			for (int pos=0; pos< nCameras; pos++){
 				int colour;
-				jderobot::ImageDataPtr dataDEPTH = sources[pos].cDEPTHprx->getImageData();
-				colorspaces::Image::FormatPtr fmt2 = colorspaces::Image::Format::searchFormat(dataDEPTH->description->format);
-				if (!fmt2)
-					throw "Format not supported";
-				colorspaces::Image imageDEPTH(dataDEPTH->description->width, dataDEPTH->description->height, fmt2, &(dataDEPTH->pixelData[0]));
-				jderobot::ImageDataPtr dataRGB = sources[pos].cRGBprx->getImageData();
-				colorspaces::Image::FormatPtr fmt = colorspaces::Image::Format::searchFormat(dataRGB->description->format);
-				if (!fmt)
-					throw "Format not supported";
-				colorspaces::Image image(dataRGB->description->width, dataRGB->description->height, fmt, &(dataRGB->pixelData[0]));
+				cv::Mat srcDEPTH =sources[cam].DEPTH->getImage();
+
+				cv::Mat distance(srcDEPTH.rows, srcDEPTH.cols, CV_32FC1);
+				//split channels to separate distance from image
+				std::vector<cv::Mat> layers;
+				cv::split(srcDEPTH, layers);
+
+				for (int x=0; x< layers[1].cols ; x++){
+					for (int y=0; y<layers[1].rows; y++){
+						distance.at<float>(y,x) = ((int)layers[1].at<unsigned char>(y,x)<<8)|(int)layers[2].at<unsigned char>(y,x);					}
+				}
+				cv::Mat rgb = sources[cam].RGB->getImage();
 				if (this->trueColor)
 					colour=0;
 				else
 					colour=pos+1;
-				this->controller->add_depth_pointsImage(imageDEPTH,image, world, pos,10,colour);
+				this->controller->add_depth_pointsImage(distance,rgb, world, pos,10,colour);
 			}
 			mainwindow->resize(1,1);
+			world->my_expose_event();
 			while (gtkmain.events_pending()){
       			gtkmain.iteration();
 	}
 		}
 		else{
 			this->world->clear_points();
-			jderobot::ImageDataPtr dataDEPTH = sources[cam].cDEPTHprx->getImageData();
-			colorspaces::Image::FormatPtr fmt2 = colorspaces::Image::Format::searchFormat(dataDEPTH->description->format);
-			if (!fmt2)
-				throw "Format not supported";
-			colorspaces::Image imageDEPTH(dataDEPTH->description->width, dataDEPTH->description->height, fmt2, &(dataDEPTH->pixelData[0]));
-			jderobot::ImageDataPtr dataRGB = sources[cam].cRGBprx->getImageData();
-			colorspaces::Image::FormatPtr fmt = colorspaces::Image::Format::searchFormat(dataRGB->description->format);
-			if (!fmt)
-				throw "Format not supported";
-			colorspaces::Image image(dataRGB->description->width, dataRGB->description->height, fmt, &(dataRGB->pixelData[0]));
-			
-			this->controller->add_depth_pointsImage(imageDEPTH,image, world, cam,5,0);
-			mainwindow->resize(1,1);
-			while (gtkmain.events_pending()){
-      			gtkmain.iteration();
-	}
+			cv::Mat srcDEPTH =sources[cam].DEPTH->getImage();
+
+			cv::Mat distance(srcDEPTH.rows, srcDEPTH.cols, CV_32FC1);
+			//split channels to separate distance from image
+			std::vector<cv::Mat> layers;
+			cv::split(srcDEPTH, layers);
+
+			for (int x=0; x< layers[1].cols ; x++){
+				for (int y=0; y<layers[1].rows; y++){
+					distance.at<float>(y,x) = ((int)layers[1].at<unsigned char>(y,x)<<8)|(int)layers[2].at<unsigned char>(y,x);					}
+			}
+
+			cv::Mat rgb = sources[cam].RGB->getImage();
+			this->controller->add_depth_pointsImage(distance,rgb, world, cam,5,0);
+
+
+			world->my_expose_event();
+
 		}
-    
 
 		/*Show window*/
-    
-    
+
   }
 
 



More information about the Jderobot-admin mailing list