[Jderobot-admin] jderobot-r1121 - trunk/src/stable/components/rgbdViewer

frivas en jderobot.org frivas en jderobot.org
Vie Nov 29 14:17:29 CET 2013


Author: frivas
Date: 2013-11-29 14:17:28 +0100 (Fri, 29 Nov 2013)
New Revision: 1121

Modified:
   trunk/src/stable/components/rgbdViewer/drawarea.cpp
   trunk/src/stable/components/rgbdViewer/drawarea.h
   trunk/src/stable/components/rgbdViewer/myprogeo.cpp
   trunk/src/stable/components/rgbdViewer/rgbdViewer.cfg
   trunk/src/stable/components/rgbdViewer/rgbdViewer.cpp
   trunk/src/stable/components/rgbdViewer/rgbdViewergui.cpp
   trunk/src/stable/components/rgbdViewer/rgbdViewergui.glade
   trunk/src/stable/components/rgbdViewer/rgbdViewergui.h
   trunk/src/stable/components/rgbdViewer/util3d.cpp
   trunk/src/stable/components/rgbdViewer/util3d.h
Log:
#137 rgbdViewer with new distance tools


Modified: trunk/src/stable/components/rgbdViewer/drawarea.cpp
===================================================================
--- trunk/src/stable/components/rgbdViewer/drawarea.cpp	2013-11-29 11:31:57 UTC (rev 1120)
+++ trunk/src/stable/components/rgbdViewer/drawarea.cpp	2013-11-29 13:17:28 UTC (rev 1121)
@@ -34,6 +34,7 @@
 	DrawArea::DrawArea(BaseObjectType* cobject, const Glib::RefPtr<Gnome::Glade::Xml>& builder)
 	: Gtk::DrawingArea(cobject), Gtk::GL::Widget<DrawArea>() {
 
+
 		this->refresh_time = 50; //ms
 		this->numlines=0;
 		this->post_p=0;
@@ -86,12 +87,12 @@
  		glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE);
 
 		/*GL Camera Position and FOA*/
-		this->glcam_pos.X=-100.;
-		this->glcam_pos.Y=-100.;
-		this->glcam_pos.Z=200.;
-		this->glcam_foa.X=100;
-		this->glcam_foa.Y=100.;
-		this->glcam_foa.Z=20.;
+		this->glcam_pos.x=-100.;
+		this->glcam_pos.y=-100.;
+		this->glcam_pos.z=200.;
+		this->glcam_foa.x=100;
+		this->glcam_foa.y=100.;
+		this->glcam_foa.z=20.;
 
     this->scale = 50;
     this->radius = 20.0;
@@ -105,43 +106,43 @@
 	DrawArea::~DrawArea() {	}
 
 	void DrawArea::setToCamera1 () {
-		this->glcam_pos.X=0.;
-		this->glcam_pos.Y=(int)MAXWORLD*10/1.2;
-		this->glcam_pos.Z=150.;
+		this->glcam_pos.x=0.;
+		this->glcam_pos.y=(int)MAXWORLD*10/1.2;
+		this->glcam_pos.z=150.;
 
-		this->glcam_foa.X=0.;
-		this->glcam_foa.Y=0.;
-		this->glcam_foa.Z=0.;
+		this->glcam_foa.x=0.;
+		this->glcam_foa.y=0.;
+		this->glcam_foa.z=0.;
 	}
 
 	void DrawArea::setToCamera2 () {
-		this->glcam_pos.X=-(int)MAXWORLD*10/1.2;
-		this->glcam_pos.Y=0.;
-		this->glcam_pos.Z=150.;
+		this->glcam_pos.x=-(int)MAXWORLD*10/1.2;
+		this->glcam_pos.y=0.;
+		this->glcam_pos.z=150.;
 
-		this->glcam_foa.X=0.;
-		this->glcam_foa.Y=0.;
-		this->glcam_foa.Z=0.;
+		this->glcam_foa.x=0.;
+		this->glcam_foa.y=0.;
+		this->glcam_foa.z=0.;
 	}
 
 	void DrawArea::setToCamera3 () {
-		this->glcam_pos.X=0.;
-		this->glcam_pos.Y=-(int)MAXWORLD*10/1.2;
-		this->glcam_pos.Z=150.;
+		this->glcam_pos.x=0.;
+		this->glcam_pos.y=-(int)MAXWORLD*10/1.2;
+		this->glcam_pos.z=150.;
 
-		this->glcam_foa.X=0.;
-		this->glcam_foa.Y=0.;
-		this->glcam_foa.Z=0.;
+		this->glcam_foa.x=0.;
+		this->glcam_foa.y=0.;
+		this->glcam_foa.z=0.;
 	}
 
 	void DrawArea::setToCamera4 () {
-		this->glcam_pos.X=(int)MAXWORLD*10/1.2;
-		this->glcam_pos.Y=0.;
-		this->glcam_pos.Z=150.;
+		this->glcam_pos.x=(int)MAXWORLD*10/1.2;
+		this->glcam_pos.y=0.;
+		this->glcam_pos.z=150.;
 
-		this->glcam_foa.X=0.;
-		this->glcam_foa.Y=0.;
-		this->glcam_foa.Z=0.;
+		this->glcam_foa.x=0.;
+		this->glcam_foa.y=0.;
+		this->glcam_foa.z=0.;
 	}
 
 	void DrawArea::readFile(std::string path){
@@ -149,12 +150,12 @@
 	}
 
 	void DrawArea::setToPioneerCamera () {
-		this->glcam_pos.X = ((10000.)*(cos(this->robottheta)) - (0.*sin(this->robottheta)) + this->robotx)/100.;
-		this->glcam_pos.Y = ((10000.)*(-sin(this->robottheta)) + (0.+cos(this->robottheta)) + this->roboty)/100.;
-		this->glcam_pos.Z = 150.;
-		this->glcam_foa.X = this->robotx/100.;
-		this->glcam_foa.Y = this->roboty/100.;
-		this->glcam_foa.Z = 0.;
+		this->glcam_pos.x = ((10000.)*(cos(this->robottheta)) - (0.*sin(this->robottheta)) + this->robotx)/100.;
+		this->glcam_pos.y = ((10000.)*(-sin(this->robottheta)) + (0.+cos(this->robottheta)) + this->roboty)/100.;
+		this->glcam_pos.z = 150.;
+		this->glcam_foa.x = this->robotx/100.;
+		this->glcam_foa.y = this->roboty/100.;
+		this->glcam_foa.z = 0.;
 	}
 
 	void DrawArea::InitOGL (int w, int h) {
@@ -223,8 +224,8 @@
 		glLoadIdentity();
 
 		/*pos cam		pto central	vector up*/
-		gluLookAt(this->glcam_pos.X, this->glcam_pos.Y, this->glcam_pos.Z,
-							this->glcam_foa.X, this->glcam_foa.Y, this->glcam_foa.Z,
+		gluLookAt((double)this->glcam_pos.x, (double)this->glcam_pos.y, (double)this->glcam_pos.z,
+							(double)this->glcam_foa.x, (double)this->glcam_foa.y, (double)this->glcam_foa.z,
 							0., 0., 1.);
 
 		/*Draw world*/
@@ -424,9 +425,9 @@
 			if ((y - old_y) > 0.0) lati += desp;
 			else if ((y - old_y) < 0.0) lati -= desp;
 
-			this->glcam_pos.X=radius*cosf(lati)*cosf(longi) + this->glcam_foa.X;
-			this->glcam_pos.Y=radius*cosf(lati)*sinf(longi) + this->glcam_foa.Y;
-			this->glcam_pos.Z=radius*sinf(lati) + this->glcam_foa.Z;
+			this->glcam_pos.x=radius*cosf(lati)*cosf(longi) + this->glcam_foa.x;
+			this->glcam_pos.y=radius*cosf(lati)*sinf(longi) + this->glcam_foa.y;
+			this->glcam_pos.z=radius*sinf(lati) + this->glcam_foa.z;
 		}
 
 		/* if right mouse button is toggled */
@@ -437,9 +438,9 @@
 			if ((y - old_y) > 0.0) lati += desp;
 			else if ((y - old_y) < 0.0) lati -= desp;
 
-			this->glcam_foa.X=-radius*cosf(lati)*cosf(longi) + this->glcam_pos.X;
-			this->glcam_foa.Y=-radius*cosf(lati)*sinf(longi) + this->glcam_pos.Y;
-			this->glcam_foa.Z=-radius*sinf(lati) + this->glcam_pos.Z;
+			this->glcam_foa.x=-radius*cosf(lati)*cosf(longi) + this->glcam_pos.x;
+			this->glcam_foa.y=-radius*cosf(lati)*sinf(longi) + this->glcam_pos.y;
+			this->glcam_foa.z=-radius*sinf(lati) + this->glcam_pos.z;
 		}
 
 		old_x=x;
@@ -450,28 +451,28 @@
 	bool DrawArea::on_drawarea_scroll(GdkEventScroll * event) {
 		float vx, vy, vz;
 
-		vx = (this->glcam_foa.X - this->glcam_pos.X)/radius;
-		vy = (this->glcam_foa.Y - this->glcam_pos.Y)/radius;
-		vz = (this->glcam_foa.Z - this->glcam_pos.Z)/radius;
+		vx = (this->glcam_foa.x - this->glcam_pos.x)/radius;
+		vy = (this->glcam_foa.y - this->glcam_pos.y)/radius;
+		vz = (this->glcam_foa.z - this->glcam_pos.z)/radius;
 
 		if (event->direction == GDK_SCROLL_UP) {
-			this->glcam_foa.X = this->glcam_foa.X + vx;
-			this->glcam_foa.Y = this->glcam_foa.Y + vy;
-			this->glcam_foa.Z = this->glcam_foa.Z + vz;
+			this->glcam_foa.x = this->glcam_foa.x + vx;
+			this->glcam_foa.y = this->glcam_foa.y + vy;
+			this->glcam_foa.z = this->glcam_foa.z + vz;
 
-			this->glcam_pos.X = this->glcam_pos.X + vx;
-			this->glcam_pos.Y = this->glcam_pos.Y + vy;
-			this->glcam_pos.Z = this->glcam_pos.Z + vz;
+			this->glcam_pos.x = this->glcam_pos.x + vx;
+			this->glcam_pos.y = this->glcam_pos.y + vy;
+			this->glcam_pos.z = this->glcam_pos.z + vz;
 		}
 
 		if (event->direction == GDK_SCROLL_DOWN) {
-			this->glcam_foa.X = this->glcam_foa.X - vx;
-			this->glcam_foa.Y = this->glcam_foa.Y - vy;
-			this->glcam_foa.Z = this->glcam_foa.Z - vz;
+			this->glcam_foa.x = this->glcam_foa.x - vx;
+			this->glcam_foa.y = this->glcam_foa.y - vy;
+			this->glcam_foa.z = this->glcam_foa.z - vz;
 
-			this->glcam_pos.X = this->glcam_pos.X - vx;
-			this->glcam_pos.Y = this->glcam_pos.Y - vy;
-			this->glcam_pos.Z = this->glcam_pos.Z - vz;
+			this->glcam_pos.x = this->glcam_pos.x - vx;
+			this->glcam_pos.y = this->glcam_pos.y - vy;
+			this->glcam_pos.z = this->glcam_pos.z - vz;
 		}
 		return true;
 	}
@@ -649,7 +650,7 @@
 	int i;
 
 	if ((myfile=fopen(worldfile,"r"))==NULL){
-		printf("rgbdViewer: cannot find config file\n");
+		std::cout << "rgbdViewer: cannot find world file" << std::endl;
 		return -1;
 	}
 

Modified: trunk/src/stable/components/rgbdViewer/drawarea.h
===================================================================
--- trunk/src/stable/components/rgbdViewer/drawarea.h	2013-11-29 11:31:57 UTC (rev 1120)
+++ trunk/src/stable/components/rgbdViewer/drawarea.h	2013-11-29 13:17:28 UTC (rev 1121)
@@ -35,8 +35,10 @@
 #include <GL/gl.h>
 #include <GL/glu.h>
 #include <GL/glut.h>
-#include <progeo/progeo.h>
 #include <jderobot/pointcloud.h>
+#include <cv.h>
+
+
 #define v3f glVertex3f
 #define IMAGE_WIDTH 320
 #define IMAGE_HEIGHT 240
@@ -120,9 +122,9 @@
 
 		SofReference mypioneer;
 		int refresh_time;
-		HPoint3D glcam_pos;
-		HPoint3D glcam_foa;
-		HPoint3D cam_pos;
+		cv::Point3f glcam_pos;
+		cv::Point3f glcam_foa;
+		cv::Point3f cam_pos;
 		
 		int numpoints;
 		int numextra_lines;

Modified: trunk/src/stable/components/rgbdViewer/myprogeo.cpp
===================================================================
--- trunk/src/stable/components/rgbdViewer/myprogeo.cpp	2013-11-29 11:31:57 UTC (rev 1120)
+++ trunk/src/stable/components/rgbdViewer/myprogeo.cpp	2013-11-29 13:17:28 UTC (rev 1121)
@@ -31,7 +31,6 @@
 	}
 	this->w=w;
 	this->h=h;
-	std::cout << "CREADO" << std::endl;
 }
 
 myprogeo::~myprogeo(){

Modified: trunk/src/stable/components/rgbdViewer/rgbdViewer.cfg
===================================================================
--- trunk/src/stable/components/rgbdViewer/rgbdViewer.cfg	2013-11-29 11:31:57 UTC (rev 1120)
+++ trunk/src/stable/components/rgbdViewer/rgbdViewer.cfg	2013-11-29 13:17:28 UTC (rev 1121)
@@ -1,18 +1,23 @@
 rgbdViewer.CameraRGBActive=1
-rgbdViewer.CameraRGB.Proxy=cameraA:tcp -h 127.0.0.1 -p 9999
-rgbdViewer.CameraDEPTHActive=0
-rgbdViewer.CameraDEPTH.Proxy=cameraB:tcp -h 127.0.0.1 -p 9998
-rgbdViewer.pointCloudActive=0
-rgbdViewer.pointCloud.Proxy=pointcloud1:tcp -h 127.0.0.1 -p 9998
+#rgbdViewer.CameraRGB.Proxy=cameraA:tcp -h 193.147.14.20 -p 9998
+rgbdViewer.CameraRGB.Fps=10
+rgbdViewer.CameraRGB.Proxy=cameraA:tcp -h localhost -p 9999
+rgbdViewer.CameraDEPTHActive=1
+rgbdViewer.CameraDEPTH.Fps=10
+#rgbdViewer.CameraDEPTH.Proxy=cameraB:tcp -h 193.147.14.20 -p 9998
+rgbdViewer.CameraDEPTH.Proxy=cameraB:tcp -h localhost -p 9999
+rgbdViewer.pointCloudActive=1
+#rgbdViewer.pointCloud.Proxy=pointcloud1:tcp -h 193.147.14.20 -p 9998
+rgbdViewer.pointCloud.Fps=10
+rgbdViewer.pointCloud.Proxy=pointcloud1:tcp -h localhost -p 9999
 rgbdViewer.Pose3DMotorsActive=0
-rgbdViewer.Pose3DMotors.Proxy=Pose3DMotors1:tcp -h 127.0.0.1 -p 9998
+rgbdViewer.Pose3DMotors.Proxy=Pose3DMotors1:tcp -h 193.147.14.20 -p 9999
 rgbdViewer.KinectLedsActive=0
-rgbdViewer.KinectLeds.Proxy=kinectleds1:tcp -h 127.0.0.1 -p 9998
-rgbdViewer.WorldFile=./config/lambecom.cfg
-#rgbdViewer.camRGB=camera-0.cfg
-#rgbdViewer.camIR=camera-0.cfg
-rgbdViewer.camRGB=cameraB.xml
-rgbdViewer.camIR=cameraB.xml
+rgbdViewer.KinectLeds.Proxy=kinectleds1:tcp -h 193.147.14.20 -p 9999
+rgbdViewer.WorldFile=./config/fempsa/fempsa.cfg
+#rgbdViewer.camRGB=./config/joseMaria/CameraAEsquina.xml
+#rgbdViewer.camIR=./config/joseMaria/CameraAEsquina.xml
 rgbdViewer.Width=320
 rgbdViewer.Height=240
 rgbdViewer.Fps=15
+rgbdViewer.Debug=0

Modified: trunk/src/stable/components/rgbdViewer/rgbdViewer.cpp
===================================================================
--- trunk/src/stable/components/rgbdViewer/rgbdViewer.cpp	2013-11-29 11:31:57 UTC (rev 1120)
+++ trunk/src/stable/components/rgbdViewer/rgbdViewer.cpp	2013-11-29 13:17:28 UTC (rev 1121)
@@ -41,30 +41,24 @@
 jderobot::cameraClient* camRGB=NULL;
 jderobot::cameraClient* camDEPTH=NULL;
 jderobot::pointcloudClient* pcClient=NULL;
+int debug;
 
 
 
-
 void *gui_thread(void* arg){
 	try{
 		//local data
 		std::vector<jderobot::RGBPoint> cloud;
 		cv::Mat rgb,depth;
 
-		struct timeval post;
-		long long int totalpre=0;
-		long long int totalpost=0;
+		IceUtil::Time lastIT;
 
-		//std::cout << "******************************** entro" << std::endl;
+		lastIT=IceUtil::Time::now();
 		while(rgbdViewergui_ptx->isVisible() && ! rgbdViewergui_ptx->isClosed()){
-			//std::cout << "******************************** entro1" << std::endl;
-			gettimeofday(&post,NULL);
-			totalpost=post.tv_sec*1000000+post.tv_usec;
-
 			if (camRGB!=NULL)
-				rgb=camRGB->getImage();
+				camRGB->getImage().copyTo(rgb);
 			if (camDEPTH!=NULL)
-				depth=camDEPTH->getImage();
+				camDEPTH->getImage().copyTo(depth);
 			if (pcClient!=NULL)
 				cloud=pcClient->getData();
 
@@ -81,15 +75,14 @@
 			else{	
 				rgbdViewergui_ptx->updatePointCloud(cloud);
 			}
-			if (totalpre !=0){
-				if ((totalpost - totalpre) > rgbdViewergui_ptx->getCycle() ){
+			if (((IceUtil::Time::now().toMicroSeconds() - lastIT.toMicroSeconds()) > rgbdViewergui_ptx->getCycle() )){
+				if (debug)
 					std::cout<<"-------- rgbdViewer: timeout-" << std::endl;
-				}
-				else{
-					usleep(rgbdViewergui_ptx->getCycle() - (totalpost - totalpre));
-				}
 			}
-			totalpre=totalpost;
+			else{
+				usleep(rgbdViewergui_ptx->getCycle() - (IceUtil::Time::now().toMicroSeconds() - lastIT.toMicroSeconds()));
+			}
+			lastIT=IceUtil::Time::now();
 
 
 		}
@@ -146,6 +139,9 @@
 			camRGB->start();
 			create_gui=true;
 		}
+		else{
+			throw "rgbdViewer: failed to load RGB Camera";
+		}
 
 	}
 	if (prop->getPropertyAsIntWithDefault("rgbdViewer.CameraDEPTHActive",0)){
@@ -155,6 +151,9 @@
 			camDEPTH->start();
 			create_gui=true;
 		}
+		else{
+			throw "rgbdViewer: failed to load ÇDEPTH Camera";
+		}
 	}
 
 
@@ -165,18 +164,20 @@
 			pointCloudSelected=true;
 			create_gui=true;
 		}
+		else{
+			throw "rgbdViewer: failed to load pointCloud";
+		}
 	}
 
 	globalHeight=prop->getPropertyAsIntWithDefault("rgbdViewer.Height",240);
 	globalWidth=prop->getPropertyAsIntWithDefault("rgbdViewer.Width",320);
-	int fps=prop->getPropertyAsIntWithDefault("rgbdViewer.Fps",10);
+	debug=prop->getPropertyAsIntWithDefault("rgbdViewer.Debug",320);
+	int fps=prop->getPropertyAsIntWithDefault("rgbdViewer.Fps",0);
 	float cycle=(float)(1/(float)fps)*1000000;
 
-
-	std::cout << rgbCamSelected <<", " << depthCamSelected << ", " << pointCloudSelected << std::endl;
-
 	rgbdViewergui_ptx = new rgbdViewer::rgbdViewergui(rgbCamSelected,depthCamSelected, pointCloudSelected, prop->getProperty("rgbdViewer.WorldFile"), prop->getProperty("rgbdViewer.camRGB"), prop->getProperty("rgbdViewer.camIR"),globalWidth,globalHeight, cycle);
 	
+	std::cout << create_gui << std::endl;
 	if (create_gui){
 		pthread_create(&threads[n_components], &attr, gui_thread,NULL);
 		n_components++;
@@ -196,7 +197,7 @@
 	if (pcClient!=NULL)
 		delete pcClient;
 
-
+	std::cout << "final" << std::endl;
 	if (ic)
 		ic->destroy();
 	return status;

Modified: trunk/src/stable/components/rgbdViewer/rgbdViewergui.cpp
===================================================================
--- trunk/src/stable/components/rgbdViewer/rgbdViewergui.cpp	2013-11-29 11:31:57 UTC (rev 1120)
+++ trunk/src/stable/components/rgbdViewer/rgbdViewergui.cpp	2013-11-29 13:17:28 UTC (rev 1121)
@@ -21,6 +21,8 @@
 
 #include "rgbdViewergui.h"
 #include <jderobot/pointcloud.h>
+#include <string>
+#include <iostream>
 
 namespace rgbdViewer {
 rgbdViewergui::rgbdViewergui(bool rgb, bool depth,bool pointCloud , std::string path, std::string path_rgb, std::string path_ir, int width, int height, float cycle): gtkmain(0,0) {
@@ -97,7 +99,12 @@
     refXml->get_widget("window_gl",w_window_gl);
     refXml->get_widget("tg_gl",w_tg_gl);
     refXml->get_widget("vbox_gl",w_vbox_gl);
+    refXml->get_widget("tg_RealDistance",w_realDistance);
+    refXml->get_widget("vbox_images_tool",w_images_tool);
+    refXml->get_widget("entry1",w_entry);
+    refXml->get_widget("labelDistance",w_Distance);
 
+
     if (!cam_rgb_active) {
         w_toggle_rgb->hide();
     }
@@ -137,6 +144,7 @@
     /*Show window. Note: Set window visibility to false in Glade, otherwise opengl won't work*/
     world->readFile(path);
     mainwindow->show();
+    this->distance = new cv::Mat(cv::Size(width,height),CV_32FC1,cv::Scalar(0,0,0));
 
 }
 
@@ -148,9 +156,11 @@
 void
 rgbdViewergui::updateAll( cv::Mat imageRGB, cv::Mat imageDEPTH, std::vector<jderobot::RGBPoint> cloud )
 {
-    //std::cout << imageRGB.rows << std::endl;
 
-    cv::Mat distance(imageRGB.rows, imageRGB.cols, CV_32FC1);
+	imageRGBlocal=imageRGB;
+	imageDEPTHlocal=imageDEPTH;
+
+
     cv::Mat colorDepth(imageDEPTH.size(),imageDEPTH.type());
     CvPoint pt1,pt2;
     if (w_toggle_rgb->get_active()) {
@@ -161,8 +171,7 @@
             //util->draw_room(imageRGB,0, world->lines, world->numlines);
         }
         w_imageRGB->set(imgBuff);
-        while (gtkmain.events_pending())
-            gtkmain.iteration();
+
     }
     if (w_toggle_depth->get_active()||((reconstruct_depth_activate)&&(reconstructMode==0))) {
 
@@ -174,11 +183,13 @@
         /*cv::imshow("color", colorDepth);
         cv::waitKey(1);*/
 
+        this->m_distance.lock();
         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);
+                distance->at<float>(y,x) = ((int)layers[1].at<unsigned char>(y,x)<<8)|(int)layers[2].at<unsigned char>(y,x);
             }
         }
+        this->m_distance.unlock();
 
         if (w_toggle_depth->get_active()) {
             cv::Mat localDepth;
@@ -189,12 +200,14 @@
             }
             w_imageDEPTH->set(imgBuff);
         }
-        while (gtkmain.events_pending())
-            gtkmain.iteration();
+
+
+
+
     }
     if (reconstruct_depth_activate) {
         if (reconstructMode==0) {
-            add_depth_pointsImage(imageRGB, distance);
+            add_depth_pointsImage(imageRGB, *distance);
         }
         else
             add_depth_pointsCloud(cloud);
@@ -234,6 +247,7 @@
 void
 rgbdViewergui::updateDEPTH(cv::Mat imageDEPTH )
 {
+	imageDEPTHlocal=imageDEPTH;
     CvPoint pt1,pt2;
     if (w_toggle_depth->get_active()) {
         Glib::RefPtr<Gdk::Pixbuf> imgBuff =  Gdk::Pixbuf::create_from_data((const guint8*) imageDEPTH.data,Gdk::COLORSPACE_RGB,false,8,imageDEPTH.cols,imageDEPTH.rows,imageDEPTH.step);
@@ -243,9 +257,6 @@
 
         }
         w_imageDEPTH->set(imgBuff);
-        displayFrameRate();
-        while (gtkmain.events_pending())
-            gtkmain.iteration();
     }
     if (reconstruct_depth_activate) {
         if (reconstructMode!=0) {
@@ -306,7 +317,6 @@
     int x,y;
     float xp,yp,zp,camx,camy,camz;
     float xu,yu,zu;
-    float k;
 
     gdk_window_at_pointer(&x,&y);
     mypro->mybackproject(x, y, &xp, &yp, &zp, &camx, &camy, &camz,0);
@@ -314,8 +324,64 @@
     yu=(yp-camy)/sqrt((xp-camx)*(xp-camx) + (yp-camy)*(yp-camy) + (zp-camz)*(zp-camz));
     zu=(zp-camz)/sqrt((xp-camx)*(xp-camx) + (yp-camy)*(yp-camy) + (zp-camz)*(zp-camz));
 
-    k= 5000;
-    world->add_line(camx+k*xu,camy+k*yu,camz+k*zu,camx,camy,camz);
+    this->m_distance.lock();
+	double d=this->distance->at<float>(y,x);
+	this->m_distance.unlock();
+	if (d!=0) {
+		//std::cout << d << std::endl;
+		//d=d*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;
+
+		mypro->mybackproject(x, y, &xp, &yp, &zp, &camx, &camy, &camz,0);
+
+
+
+		//vector unitario
+		float modulo;
+
+		modulo = sqrt(1/(((camx-xp)*(camx-xp))+((camy-yp)*(camy-yp))+((camz-zp)*(camz-zp))));
+		mypro->mygetcamerafoa(&c1x, &c1y, &c1z, 0);
+
+
+		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= 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));
+		//imprimos la información
+		std::stringstream ss;
+		ss << d;
+		w_Distance->set_text(ss.str());
+
+	}
+	else{
+
+		d= 5000;
+		w_Distance->set_text("no measure");
+	}
+
+
+
+
+    world->add_line(camx+d*xu,camy+d*yu,camz+d*zu,camx,camy,camz);
     return true;
 }
 
@@ -324,7 +390,6 @@
     int x,y;
     float xp,yp,zp,camx,camy,camz;
     float xu,yu,zu;
-    float k;
 
     gdk_window_at_pointer(&x,&y);
     mypro->mybackproject(x, y, &xp, &yp, &zp, &camx, &camy, &camz,1);
@@ -332,10 +397,67 @@
     yu=(yp-camy)/sqrt((xp-camx)*(xp-camx) + (yp-camy)*(yp-camy) + (zp-camz)*(zp-camz));
     zu=(zp-camz)/sqrt((xp-camx)*(xp-camx) + (yp-camy)*(yp-camy) + (zp-camz)*(zp-camz));
 
-    k= 5000;
-    world->add_line(camx+k*xu,camy+k*yu,camz+k*zu,camx,camy,camz);
+    this->m_distance.lock();
+	double d=this->distance->at<float>(y,x);
+	this->m_distance.unlock();
+	if (d!=0) {
+		//std::cout << d << std::endl;
+		//d=d*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;
 
-    world->add_line(k*xu + camx,k*yu + camy,k*zu + camz,camx,camy,camz);
+		mypro->mybackproject(x, y, &xp, &yp, &zp, &camx, &camy, &camz,0);
+
+
+
+		//vector unitario
+		float modulo;
+
+		modulo = sqrt(1/(((camx-xp)*(camx-xp))+((camy-yp)*(camy-yp))+((camz-zp)*(camz-zp))));
+		mypro->mygetcamerafoa(&c1x, &c1y, &c1z, 0);
+
+
+		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= 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));
+		//imprimos la información
+		std::stringstream ss;
+		ss << d;
+		w_Distance->set_text(ss.str());
+
+	}
+	else{
+
+		d= 5000;
+		w_Distance->set_text("no measure");
+	}
+
+
+
+
+
+    world->add_line(camx+d*xu,camy+d*yu,camz+d*zu,camx,camy,camz);
+
+    world->add_line(d*xu + camx,d*yu + camy,d*zu + camz,camx,camy,camz);
     return true;
 }
 
@@ -376,12 +498,17 @@
 void
 rgbdViewergui::add_depth_pointsImage(cv::Mat imageRGB, cv::Mat distance) {
     float d;
-    //std::cout << "point image" << std::endl;
 
     world->clear_points();
     //std::cout << "inicio reconstrucción" << std::endl;
-    for (int xIm=0; xIm< cWidth; xIm++) {
-        for (int yIm=0; yIm<cHeight ; yIm++) {
+
+    int sampling= atoi(this->w_entry->get_buffer()->get_text().c_str());
+
+    if (sampling<=0){
+    	sampling=1;
+    }
+    for (int xIm=0; xIm< cWidth; xIm+=sampling) {
+        for (int yIm=0; yIm<cHeight ; yIm+=sampling) {
             d=distance.at<float>(yIm,xIm);
             if (d!=0) {
                 //std::cout << d << std::endl;
@@ -396,9 +523,6 @@
                 float t;
                 float Fx,Fy,Fz;
 
-
-		
-
                 mypro->mybackproject(xIm, yIm, &xp, &yp, &zp, &camx, &camy, &camz,0);
 
 			
@@ -427,18 +551,13 @@
 
 
 
-                /*world->points[i][0]=distance*ux+camx;
-                world->points[i][1]=distance*uy+camy;
-                world->points[i][2]=distance*uz+camz;*/
-                //std::cout << c1x << "," << c1y << "," << c1f << "," << 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;
-                //std::cout<< t*ux + camx << ", " << t*uy + camy << ", " << t*uz + camz << std::endl;
-                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]);
+                if (w_realDistance->get_active()){
+                	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{
+                	world->add_kinect_point(d*ux + camx,d*uy+ camy,d*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]);
+                }
 
-                //world->add_line(distance*ux + camx,distance*uy+ camy,distance*uz + camz,camx,camy,camz);
             }
         }
     }
@@ -564,14 +683,18 @@
 
 void
 rgbdViewergui::on_w_radio_mode_pointcloud_activate() {
-    if (w_radio_mode_pointcloud->get_active())
+    if (w_radio_mode_pointcloud->get_active()){
         reconstructMode=1;
+        w_images_tool->hide();
+    }
 }
 
 void
 rgbdViewergui::on_w_radio_mode_image_activate() {
-    if (w_radio_mode_image->get_active())
+    if (w_radio_mode_image->get_active()){
         reconstructMode=0;
+        w_images_tool->show();
+    }
 }
 
 void

Modified: trunk/src/stable/components/rgbdViewer/rgbdViewergui.glade
===================================================================
--- trunk/src/stable/components/rgbdViewer/rgbdViewergui.glade	2013-11-29 11:31:57 UTC (rev 1120)
+++ trunk/src/stable/components/rgbdViewer/rgbdViewergui.glade	2013-11-29 13:17:28 UTC (rev 1121)
@@ -3,25 +3,31 @@
   <!-- interface-requires gtk+ 2.16 -->
   <!-- interface-naming-policy project-wide -->
   <widget class="GtkWindow" id="rgbdViewer">
+    <property name="can_focus">False</property>
     <property name="title" translatable="yes">rgbdViewer</property>
     <child>
       <widget class="GtkVBox" id="vbox1">
         <property name="visible">True</property>
+        <property name="can_focus">False</property>
         <child>
           <widget class="GtkMenuBar" id="menubar2">
             <property name="visible">True</property>
+            <property name="can_focus">False</property>
             <child>
               <widget class="GtkMenuItem" id="menuitem5">
                 <property name="visible">True</property>
+                <property name="can_focus">False</property>
                 <property name="label" translatable="yes">_File</property>
                 <property name="use_underline">True</property>
                 <child>
                   <widget class="GtkMenu" id="menu4">
                     <property name="visible">True</property>
+                    <property name="can_focus">False</property>
                     <child>
                       <widget class="GtkImageMenuItem" id="imagemenuitem11">
                         <property name="label">gtk-new</property>
                         <property name="visible">True</property>
+                        <property name="can_focus">False</property>
                         <property name="use_underline">True</property>
                         <property name="use_stock">True</property>
                       </widget>
@@ -30,6 +36,7 @@
                       <widget class="GtkImageMenuItem" id="imagemenuitem12">
                         <property name="label">gtk-open</property>
                         <property name="visible">True</property>
+                        <property name="can_focus">False</property>
                         <property name="use_underline">True</property>
                         <property name="use_stock">True</property>
                       </widget>
@@ -38,6 +45,7 @@
                       <widget class="GtkImageMenuItem" id="imagemenuitem13">
                         <property name="label">gtk-save</property>
                         <property name="visible">True</property>
+                        <property name="can_focus">False</property>
                         <property name="use_underline">True</property>
                         <property name="use_stock">True</property>
                       </widget>
@@ -46,6 +54,7 @@
                       <widget class="GtkImageMenuItem" id="imagemenuitem14">
                         <property name="label">gtk-save-as</property>
                         <property name="visible">True</property>
+                        <property name="can_focus">False</property>
                         <property name="use_underline">True</property>
                         <property name="use_stock">True</property>
                       </widget>
@@ -53,12 +62,14 @@
                     <child>
                       <widget class="GtkSeparatorMenuItem" id="separatormenuitem2">
                         <property name="visible">True</property>
+                        <property name="can_focus">False</property>
                       </widget>
                     </child>
                     <child>
                       <widget class="GtkImageMenuItem" id="imagemenuitem15">
                         <property name="label">gtk-quit</property>
                         <property name="visible">True</property>
+                        <property name="can_focus">False</property>
                         <property name="use_underline">True</property>
                         <property name="use_stock">True</property>
                       </widget>
@@ -70,15 +81,18 @@
             <child>
               <widget class="GtkMenuItem" id="menuitem6">
                 <property name="visible">True</property>
+                <property name="can_focus">False</property>
                 <property name="label" translatable="yes">_Edit</property>
                 <property name="use_underline">True</property>
                 <child>
                   <widget class="GtkMenu" id="menu5">
                     <property name="visible">True</property>
+                    <property name="can_focus">False</property>
                     <child>
                       <widget class="GtkImageMenuItem" id="imagemenuitem16">
                         <property name="label">gtk-cut</property>
                         <property name="visible">True</property>
+                        <property name="can_focus">False</property>
                         <property name="use_underline">True</property>
                         <property name="use_stock">True</property>
                       </widget>
@@ -87,6 +101,7 @@
                       <widget class="GtkImageMenuItem" id="imagemenuitem17">
                         <property name="label">gtk-copy</property>
                         <property name="visible">True</property>
+                        <property name="can_focus">False</property>
                         <property name="use_underline">True</property>
                         <property name="use_stock">True</property>
                       </widget>
@@ -95,6 +110,7 @@
                       <widget class="GtkImageMenuItem" id="imagemenuitem18">
                         <property name="label">gtk-paste</property>
                         <property name="visible">True</property>
+                        <property name="can_focus">False</property>
                         <property name="use_underline">True</property>
                         <property name="use_stock">True</property>
                       </widget>
@@ -103,6 +119,7 @@
                       <widget class="GtkImageMenuItem" id="imagemenuitem19">
                         <property name="label">gtk-delete</property>
                         <property name="visible">True</property>
+                        <property name="can_focus">False</property>
                         <property name="use_underline">True</property>
                         <property name="use_stock">True</property>
                       </widget>
@@ -114,19 +131,23 @@
             <child>
               <widget class="GtkMenuItem" id="menuitem7">
                 <property name="visible">True</property>
+                <property name="can_focus">False</property>
                 <property name="label" translatable="yes">_View</property>
                 <property name="use_underline">True</property>
                 <child>
                   <widget class="GtkMenu" id="menu7">
                     <property name="visible">True</property>
+                    <property name="can_focus">False</property>
                     <child>
                       <widget class="GtkImageMenuItem" id="view_controller">
                         <property name="label" translatable="yes">Controller</property>
                         <property name="visible">True</property>
+                        <property name="can_focus">False</property>
                         <property name="use_stock">False</property>
                         <child internal-child="image">
                           <widget class="GtkImage" id="image1">
                             <property name="visible">True</property>
+                            <property name="can_focus">False</property>
                             <property name="stock">gtk-missing-image</property>
                           </widget>
                         </child>
@@ -139,15 +160,18 @@
             <child>
               <widget class="GtkMenuItem" id="menuitem8">
                 <property name="visible">True</property>
+                <property name="can_focus">False</property>
                 <property name="label" translatable="yes">_Help</property>
                 <property name="use_underline">True</property>
                 <child>
                   <widget class="GtkMenu" id="menu6">
                     <property name="visible">True</property>
+                    <property name="can_focus">False</property>
                     <child>
                       <widget class="GtkImageMenuItem" id="imagemenuitem20">
                         <property name="label">gtk-about</property>
                         <property name="visible">True</property>
+                        <property name="can_focus">False</property>
                         <property name="use_underline">True</property>
                         <property name="use_stock">True</property>
                       </widget>
@@ -159,32 +183,40 @@
           </widget>
           <packing>
             <property name="expand">False</property>
+            <property name="fill">True</property>
             <property name="position">0</property>
           </packing>
         </child>
         <child>
           <widget class="GtkVBox" id="vbox2">
             <property name="visible">True</property>
+            <property name="can_focus">False</property>
             <child>
               <widget class="GtkHBox" id="hbox1">
                 <property name="visible">True</property>
+                <property name="can_focus">False</property>
                 <child>
                   <widget class="GtkEventBox" id="eventboxRGB">
                     <property name="visible">True</property>
+                    <property name="can_focus">False</property>
                     <child>
                       <widget class="GtkImage" id="imageRGB">
                         <property name="visible">True</property>
+                        <property name="can_focus">False</property>
                         <property name="stock">gtk-missing-image</property>
                       </widget>
                     </child>
                   </widget>
                   <packing>
+                    <property name="expand">True</property>
+                    <property name="fill">True</property>
                     <property name="position">0</property>
                   </packing>
                 </child>
                 <child>
                   <widget class="GtkVSeparator" id="vseparator1">
                     <property name="visible">True</property>
+                    <property name="can_focus">False</property>
                   </widget>
                   <packing>
                     <property name="expand">False</property>
@@ -195,23 +227,29 @@
                 <child>
                   <widget class="GtkEventBox" id="eventboxDEPTH">
                     <property name="visible">True</property>
+                    <property name="can_focus">False</property>
                     <child>
                       <widget class="GtkImage" id="imageDEPTH">
                         <property name="visible">True</property>
+                        <property name="can_focus">False</property>
                         <property name="stock">gtk-missing-image</property>
                       </widget>
                     </child>
                   </widget>
                   <packing>
+                    <property name="expand">True</property>
+                    <property name="fill">True</property>
                     <property name="position">2</property>
                   </packing>
                 </child>
                 <child>
                   <widget class="GtkVSeparator" id="vseparator2">
                     <property name="visible">True</property>
+                    <property name="can_focus">False</property>
                   </widget>
                   <packing>
                     <property name="expand">False</property>
+                    <property name="fill">True</property>
                     <property name="position">3</property>
                   </packing>
                 </child>
@@ -221,20 +259,25 @@
               </widget>
               <packing>
                 <property name="expand">False</property>
+                <property name="fill">True</property>
                 <property name="position">0</property>
               </packing>
             </child>
             <child>
               <widget class="GtkHSeparator" id="hseparator1">
                 <property name="visible">True</property>
+                <property name="can_focus">False</property>
               </widget>
               <packing>
                 <property name="expand">False</property>
+                <property name="fill">True</property>
                 <property name="position">1</property>
               </packing>
             </child>
           </widget>
           <packing>
+            <property name="expand">True</property>
+            <property name="fill">True</property>
             <property name="position">1</property>
           </packing>
         </child>
@@ -243,13 +286,16 @@
   </widget>
   <widget class="GtkWindow" id="window_controller">
     <property name="visible">True</property>
+    <property name="can_focus">False</property>
     <property name="title" translatable="yes">Controller</property>
     <child>
       <widget class="GtkVBox" id="vbox3">
         <property name="visible">True</property>
+        <property name="can_focus">False</property>
         <child>
           <widget class="GtkHBox" id="hbox2">
             <property name="visible">True</property>
+            <property name="can_focus">False</property>
             <child>
               <widget class="GtkToggleButton" id="toggle_rgb">
                 <property name="label" translatable="yes">Camera RGB</property>
@@ -258,30 +304,106 @@
                 <property name="receives_default">True</property>
               </widget>
               <packing>
+                <property name="expand">True</property>
+                <property name="fill">True</property>
                 <property name="position">0</property>
               </packing>
             </child>
             <child>
-              <widget class="GtkToggleButton" id="toggle_depth">
-                <property name="label" translatable="yes">Camera Depth</property>
+              <widget class="GtkVBox" id="vbox7">
                 <property name="visible">True</property>
-                <property name="can_focus">True</property>
-                <property name="receives_default">True</property>
+                <property name="can_focus">False</property>
+                <child>
+                  <widget class="GtkToggleButton" id="toggle_depth">
+                    <property name="label" translatable="yes">Camera Depth</property>
+                    <property name="visible">True</property>
+                    <property name="can_focus">True</property>
+                    <property name="receives_default">True</property>
+                  </widget>
+                  <packing>
+                    <property name="expand">True</property>
+                    <property name="fill">True</property>
+                    <property name="position">0</property>
+                  </packing>
+                </child>
+                <child>
+                  <widget class="GtkHSeparator" id="hseparator4">
+                    <property name="visible">True</property>
+                    <property name="can_focus">False</property>
+                  </widget>
+                  <packing>
+                    <property name="expand">False</property>
+                    <property name="fill">True</property>
+                    <property name="position">1</property>
+                  </packing>
+                </child>
+                <child>
+                  <widget class="GtkHBox" id="hbox6">
+                    <property name="visible">True</property>
+                    <property name="can_focus">False</property>
+                    <child>
+                      <widget class="GtkLabel" id="label2">
+                        <property name="visible">True</property>
+                        <property name="can_focus">False</property>
+                        <property name="label" translatable="yes">Distance:</property>
+                      </widget>
+                      <packing>
+                        <property name="expand">True</property>
+                        <property name="fill">True</property>
+                        <property name="position">0</property>
+                      </packing>
+                    </child>
+                    <child>
+                      <widget class="GtkVSeparator" id="vseparator7">
+                        <property name="visible">True</property>
+                        <property name="can_focus">False</property>
+                      </widget>
+                      <packing>
+                        <property name="expand">False</property>
+                        <property name="fill">True</property>
+                        <property name="position">1</property>
+                      </packing>
+                    </child>
+                    <child>
+                      <widget class="GtkLabel" id="labelDistance">
+                        <property name="visible">True</property>
+                        <property name="can_focus">False</property>
+                        <property name="label" translatable="yes">0</property>
+                      </widget>
+                      <packing>
+                        <property name="expand">True</property>
+                        <property name="fill">True</property>
+                        <property name="position">2</property>
+                      </packing>
+                    </child>
+                  </widget>
+                  <packing>
+                    <property name="expand">True</property>
+                    <property name="fill">True</property>
+                    <property name="position">2</property>
+                  </packing>
+                </child>
               </widget>
               <packing>
+                <property name="expand">True</property>
+                <property name="fill">True</property>
                 <property name="position">1</property>
               </packing>
             </child>
           </widget>
           <packing>
+            <property name="expand">True</property>
+            <property name="fill">True</property>
             <property name="position">0</property>
           </packing>
         </child>
         <child>
           <widget class="GtkVBox" id="vbox4">
             <property name="visible">True</property>
+            <property name="can_focus">False</property>
             <child>
               <widget class="GtkVBox" id="vbox_Pose3DMotors">
+                <property name="can_focus">False</property>
                 <child>
                   <widget class="GtkButton" id="button_up">
                     <property name="label" translatable="yes">UP</property>
@@ -290,6 +412,8 @@
                     <property name="receives_default">True</property>
                   </widget>
                   <packing>
+                    <property name="expand">True</property>
+                    <property name="fill">True</property>
                     <property name="position">0</property>
                   </packing>
                 </child>
@@ -301,16 +425,21 @@
                     <property name="receives_default">True</property>
                   </widget>
                   <packing>
+                    <property name="expand">True</property>
+                    <property name="fill">True</property>
                     <property name="position">1</property>
                   </packing>
                 </child>
               </widget>
               <packing>
+                <property name="expand">True</property>
+                <property name="fill">True</property>
                 <property name="position">0</property>
               </packing>
             </child>
             <child>
               <widget class="GtkHBox" id="hbox_leds">
+                <property name="can_focus">False</property>
                 <child>
                   <widget class="GtkButton" id="buttonled_off">
                     <property name="label" translatable="yes">OFF</property>
@@ -319,6 +448,8 @@
                     <property name="receives_default">True</property>
                   </widget>
                   <packing>
+                    <property name="expand">True</property>
+                    <property name="fill">True</property>
                     <property name="position">0</property>
                   </packing>
                 </child>
@@ -330,6 +461,8 @@
                     <property name="receives_default">True</property>
                   </widget>
                   <packing>
+                    <property name="expand">True</property>
+                    <property name="fill">True</property>
                     <property name="position">1</property>
                   </packing>
                 </child>
@@ -341,6 +474,8 @@
                     <property name="receives_default">True</property>
                   </widget>
                   <packing>
+                    <property name="expand">True</property>
+                    <property name="fill">True</property>
                     <property name="position">2</property>
                   </packing>
                 </child>
@@ -352,6 +487,8 @@
                     <property name="receives_default">True</property>
                   </widget>
                   <packing>
+                    <property name="expand">True</property>
+                    <property name="fill">True</property>
                     <property name="position">3</property>
                   </packing>
                 </child>
@@ -363,6 +500,8 @@
                     <property name="receives_default">True</property>
                   </widget>
                   <packing>
+                    <property name="expand">True</property>
+                    <property name="fill">True</property>
                     <property name="position">4</property>
                   </packing>
                 </child>
@@ -374,26 +513,33 @@
                     <property name="receives_default">True</property>
                   </widget>
                   <packing>
+                    <property name="expand">True</property>
+                    <property name="fill">True</property>
                     <property name="position">5</property>
                   </packing>
                 </child>
               </widget>
               <packing>
+                <property name="expand">True</property>
+                <property name="fill">True</property>
                 <property name="position">1</property>
               </packing>
             </child>
             <child>
               <widget class="GtkHSeparator" id="hseparator2">
                 <property name="visible">True</property>
+                <property name="can_focus">False</property>
               </widget>
               <packing>
                 <property name="expand">False</property>
+                <property name="fill">True</property>
                 <property name="position">2</property>
               </packing>
             </child>
             <child>
               <widget class="GtkVBox" id="vbox5">
                 <property name="visible">True</property>
+                <property name="can_focus">False</property>
                 <child>
                   <widget class="GtkToggleButton" id="tg_gl">
                     <property name="label" translatable="yes">View World</property>
@@ -402,14 +548,18 @@
                     <property name="receives_default">True</property>
                   </widget>
                   <packing>
+                    <property name="expand">True</property>
+                    <property name="fill">True</property>
                     <property name="position">0</property>
                   </packing>
                 </child>
                 <child>
                   <widget class="GtkVBox" id="vbox_gl">
+                    <property name="can_focus">False</property>
                     <child>
                       <widget class="GtkHBox" id="hbox4">
                         <property name="visible">True</property>
+                        <property name="can_focus">False</property>
                         <child>
                           <widget class="GtkToggleButton" id="toggle_lines_rgb">
                             <property name="label" translatable="yes">Show room on rgb</property>
@@ -418,6 +568,8 @@
                             <property name="receives_default">True</property>
                           </widget>
                           <packing>
+                            <property name="expand">True</property>
+                            <property name="fill">True</property>
                             <property name="position">0</property>
                           </packing>
                         </child>
@@ -429,11 +581,15 @@
                             <property name="receives_default">True</property>
                           </widget>
                           <packing>
+                            <property name="expand">True</property>
+                            <property name="fill">True</property>
                             <property name="position">1</property>
                           </packing>
                         </child>
                       </widget>
                       <packing>
+                        <property name="expand">True</property>
+                        <property name="fill">True</property>
                         <property name="position">0</property>
                       </packing>
                     </child>
@@ -445,12 +601,15 @@
                         <property name="receives_default">True</property>
                       </widget>
                       <packing>
+                        <property name="expand">True</property>
+                        <property name="fill">True</property>
                         <property name="position">1</property>
                       </packing>
                     </child>
                     <child>
                       <widget class="GtkHBox" id="hbox3">
                         <property name="visible">True</property>
+                        <property name="can_focus">False</property>
                         <child>
                           <widget class="GtkToggleButton" id="toggle_reconstruct">
                             <property name="label" translatable="yes">Reconstruct</property>
@@ -459,20 +618,25 @@
                             <property name="receives_default">True</property>
                           </widget>
                           <packing>
+                            <property name="expand">True</property>
+                            <property name="fill">True</property>
                             <property name="position">0</property>
                           </packing>
                         </child>
                         <child>
                           <widget class="GtkVSeparator" id="vseparator3">
                             <property name="visible">True</property>
+                            <property name="can_focus">False</property>
                           </widget>
                           <packing>
                             <property name="expand">False</property>
+                            <property name="fill">True</property>
                             <property name="position">1</property>
                           </packing>
                         </child>
                         <child>
                           <widget class="GtkVBox" id="vbox_reconstruct_mode">
+                            <property name="can_focus">False</property>
                             <child>
                               <widget class="GtkRadioButton" id="radio_mode_pointcloud">
                                 <property name="label" translatable="yes">PointCloud</property>
@@ -483,6 +647,8 @@
                                 <property name="draw_indicator">True</property>
                               </widget>
                               <packing>
+                                <property name="expand">True</property>
+                                <property name="fill">True</property>
                                 <property name="position">0</property>
                               </packing>
                             </child>
@@ -497,25 +663,32 @@
                                 <property name="group">radio_mode_pointcloud</property>
                               </widget>
                               <packing>
+                                <property name="expand">True</property>
+                                <property name="fill">True</property>
                                 <property name="position">1</property>
                               </packing>
                             </child>
                           </widget>
                           <packing>
+                            <property name="expand">True</property>
+                            <property name="fill">True</property>
                             <property name="position">2</property>
                           </packing>
                         </child>
                         <child>
                           <widget class="GtkVSeparator" id="vseparator4">
                             <property name="visible">True</property>
+                            <property name="can_focus">False</property>
                           </widget>
                           <packing>
                             <property name="expand">False</property>
+                            <property name="fill">True</property>
                             <property name="position">3</property>
                           </packing>
                         </child>
                         <child>
                           <widget class="GtkVBox" id="vbox_reconstruct_selection">
+                            <property name="can_focus">False</property>
                             <child>
                               <widget class="GtkRadioButton" id="radio_depth">
                                 <property name="label" translatable="yes">DEPTH</property>
@@ -526,6 +699,8 @@
                                 <property name="draw_indicator">True</property>
                               </widget>
                               <packing>
+                                <property name="expand">True</property>
+                                <property name="fill">True</property>
                                 <property name="position">0</property>
                               </packing>
                             </child>
@@ -539,51 +714,168 @@
                                 <property name="group">radio_depth</property>
                               </widget>
                               <packing>
+                                <property name="expand">True</property>
+                                <property name="fill">True</property>
                                 <property name="position">1</property>
                               </packing>
                             </child>
+                            <child>
+                              <widget class="GtkVBox" id="vbox_images_tool">
+                                <property name="can_focus">False</property>
+                                <child>
+                                  <widget class="GtkHBox" id="hbox5">
+                                    <property name="visible">True</property>
+                                    <property name="can_focus">False</property>
+                                    <child>
+                                      <widget class="GtkLabel" id="label1">
+                                        <property name="visible">True</property>
+                                        <property name="can_focus">False</property>
+                                        <property name="label" translatable="yes">Sampling</property>
+                                      </widget>
+                                      <packing>
+                                        <property name="expand">True</property>
+                                        <property name="fill">True</property>
+                                        <property name="position">0</property>
+                                      </packing>
+                                    </child>
+                                    <child>
+                                      <widget class="GtkVSeparator" id="vseparator6">
+                                        <property name="visible">True</property>
+                                        <property name="can_focus">False</property>
+                                      </widget>
+                                      <packing>
+                                        <property name="expand">False</property>
+                                        <property name="fill">True</property>
+                                        <property name="position">1</property>
+                                      </packing>
+                                    </child>
+                                    <child>
+                                      <widget class="GtkEntry" id="entry1">
+                                        <property name="width_request">4</property>
+                                        <property name="visible">True</property>
+                                        <property name="can_focus">True</property>
+                                        <property name="invisible_char">•</property>
+                                        <property name="text" translatable="yes">3</property>
+                                        <property name="invisible_char_set">True</property>
+                                        <property name="primary_icon_activatable">False</property>
+                                        <property name="secondary_icon_activatable">False</property>
+                                        <property name="primary_icon_sensitive">True</property>
+                                        <property name="secondary_icon_sensitive">True</property>
+                                      </widget>
+                                      <packing>
+                                        <property name="expand">True</property>
+                                        <property name="fill">True</property>
+                                        <property name="position">2</property>
+                                      </packing>
+                                    </child>
+                                  </widget>
+                                  <packing>
+                                    <property name="expand">True</property>
+                                    <property name="fill">True</property>
+                                    <property name="position">0</property>
+                                  </packing>
+                                </child>
+                                <child>
+                                  <widget class="GtkToggleButton" id="tg_RealDistance">
+                                    <property name="label" translatable="yes">Real Distance</property>
+                                    <property name="visible">True</property>
+                                    <property name="can_focus">True</property>
+                                    <property name="receives_default">True</property>
+                                  </widget>
+                                  <packing>
+                                    <property name="expand">True</property>
+                                    <property name="fill">True</property>
+                                    <property name="position">1</property>
+                                  </packing>
+                                </child>
+                              </widget>
+                              <packing>
+                                <property name="expand">True</property>
+                                <property name="fill">True</property>
+                                <property name="position">2</property>
+                              </packing>
+                            </child>
                           </widget>
                           <packing>
+                            <property name="expand">True</property>
+                            <property name="fill">True</property>
                             <property name="position">4</property>
                           </packing>
                         </child>
                         <child>
                           <widget class="GtkVSeparator" id="vseparator5">
                             <property name="visible">True</property>
+                            <property name="can_focus">False</property>
                           </widget>
                           <packing>
                             <property name="expand">False</property>
+                            <property name="fill">True</property>
                             <property name="position">5</property>
                           </packing>
                         </child>
                         <child>
-                          <widget class="GtkToggleButton" id="toggle_camera_pos">
-                            <property name="label" translatable="yes">Camera Position</property>
+                          <widget class="GtkVBox" id="vbox6">
                             <property name="visible">True</property>
-                            <property name="can_focus">True</property>
-                            <property name="receives_default">True</property>
+                            <property name="can_focus">False</property>
+                            <child>
+                              <placeholder/>
+                            </child>
+                            <child>
+                              <widget class="GtkHSeparator" id="hseparator3">
+                                <property name="visible">True</property>
+                                <property name="can_focus">False</property>
+                              </widget>
+                              <packing>
+                                <property name="expand">False</property>
+                                <property name="fill">True</property>
+                                <property name="position">1</property>
+                              </packing>
+                            </child>
+                            <child>
+                              <widget class="GtkToggleButton" id="toggle_camera_pos">
+                                <property name="label" translatable="yes">Camera Position</property>
+                                <property name="visible">True</property>
+                                <property name="can_focus">True</property>
+                                <property name="receives_default">True</property>
+                              </widget>
+                              <packing>
+                                <property name="expand">True</property>
+                                <property name="fill">True</property>
+                                <property name="position">2</property>
+                              </packing>
+                            </child>
                           </widget>
                           <packing>
+                            <property name="expand">True</property>
+                            <property name="fill">True</property>
                             <property name="position">6</property>
                           </packing>
                         </child>
                       </widget>
                       <packing>
+                        <property name="expand">True</property>
+                        <property name="fill">True</property>
                         <property name="position">2</property>
                       </packing>
                     </child>
                   </widget>
                   <packing>
+                    <property name="expand">True</property>
+                    <property name="fill">True</property>
                     <property name="position">1</property>
                   </packing>
                 </child>
               </widget>
               <packing>
+                <property name="expand">True</property>
+                <property name="fill">True</property>
                 <property name="position">3</property>
               </packing>
             </child>
           </widget>
           <packing>
+            <property name="expand">True</property>
+            <property name="fill">True</property>
             <property name="position">1</property>
           </packing>
         </child>
@@ -593,11 +885,13 @@
   <widget class="GtkWindow" id="window_gl">
     <property name="width_request">640</property>
     <property name="height_request">480</property>
+    <property name="can_focus">False</property>
     <property name="title" translatable="yes">World</property>
     <child>
       <widget class="GtkDrawingArea" id="gl_world">
         <property name="height_request">300</property>
         <property name="visible">True</property>
+        <property name="can_focus">False</property>
       </widget>
     </child>
   </widget>

Modified: trunk/src/stable/components/rgbdViewer/rgbdViewergui.h
===================================================================
--- trunk/src/stable/components/rgbdViewer/rgbdViewergui.h	2013-11-29 11:31:57 UTC (rev 1120)
+++ trunk/src/stable/components/rgbdViewer/rgbdViewergui.h	2013-11-29 13:17:28 UTC (rev 1121)
@@ -76,6 +76,7 @@
 			Gtk::ToggleButton *w_toggle_rgb;
 			Gtk::ToggleButton *w_toggle_depth;
 			Gtk::ImageMenuItem *w_view_controller;
+			Gtk::ToggleButton *w_realDistance;
 			Gtk::VBox * w_reconstruct_selection;
 			Gtk::VBox * w_reconstruct_mode;
 			Gtk::VBox * w_vbox_gl;
@@ -84,6 +85,14 @@
 			Gtk::RadioButton *w_radio_mode_pointcloud;
 			Gtk::RadioButton *w_radio_mode_image;
 			Gtk::Button *w_button_clear_lines;
+			Gtk::VBox *w_images_tool;
+			Gtk::Entry *w_entry;
+			Gtk::Label *w_Distance;
+
+			cv::Mat imageRGBlocal;
+			cv::Mat imageDEPTHlocal;
+
+
 			//util3d* util;
 			DrawArea* world;
 			int cWidth, cHeight;
@@ -120,8 +129,13 @@
 			void on_w_tg_gl_toggled();
 			
 
+			cv::Mat* distance;
+			//distance mutex
+			IceUtil::Mutex m_distance;
 
 
+
+
   };
 } // namespace
 

Modified: trunk/src/stable/components/rgbdViewer/util3d.cpp
===================================================================
--- trunk/src/stable/components/rgbdViewer/util3d.cpp	2013-11-29 11:31:57 UTC (rev 1120)
+++ trunk/src/stable/components/rgbdViewer/util3d.cpp	2013-11-29 13:17:28 UTC (rev 1121)
@@ -22,7 +22,7 @@
 #include "util3d.h"
 
 
-namespace kinectViewer{
+namespace rgbdViewer{
 
 util3d::util3d(myprogeo* p){
 	mypro = p;
@@ -31,39 +31,41 @@
 util3d::~util3d(){
 }
 
-int util3d::cvDrawline(cv::Mat image,HPoint2D p1, HPoint2D p2, cv::Scalar color,int cam){
-  HPoint2D gooda,goodb;
+int util3d::cvDrawline(cv::Mat image,Eigen::Vector3d p1, Eigen::Vector3d p2, cv::Scalar color,int cam){
+  Eigen::Vector3d gooda,goodb;
   CvPoint pt1,pt2;
-	TPinHoleCamera camera;
+  Progeo::Progeo* camera;
 
 	camera=mypro->getCamera(cam);
-  if(displayline(p1,p2,&gooda,&goodb,camera)==1){
-	pt1.x=(int)gooda.y; pt1.y=camera.rows-1-(int)gooda.x;
-	pt2.x=(int)goodb.y; pt2.y=camera.rows-1-(int)goodb.x;
+ /* if(camera->displayline(p1,p2,gooda,goodb)==1){
+	pt1.x=(int)gooda(1);
+	pt1.y=camera->getImageHeight()-1-(int)gooda(0);
+	pt2.x=(int)goodb(1);
+	pt2.y=camera->getImageWidth()-1-(int)goodb(0);
 	cv::line(image,pt1,pt2,color,2,1,0);
 	return 1;
-  }
+  }*/
   return 0;
 }
 
 void util3d::draw_room(cv::Mat image,int cam, float lines[][8], int n_lines){
   	int i,j;
-  	HPoint2D a,b;
-  	HPoint3D a3A,a3B,a3C,a3D;
-  	TPinHoleCamera camera;
+  	Eigen::Vector3d a,b;
+  	Eigen::Vector4d a3A,a3B,a3C,a3D;
+  	Progeo::Progeo* camera;
 
 	camera=mypro->getCamera(cam);
   	for(i=0;i<n_lines;i++){
-		a3A.X=lines[i][0];
-		a3A.Y=lines[i][1];
-		a3A.Z=lines[i][2];
-		a3A.H=lines[i][3];
-		a3B.X=lines[i][4];
-		a3B.Y=lines[i][5];
-		a3B.Z=lines[i][6];
-		a3B.H=lines[i][7];
-		project(a3A,&a,camera);
-		project(a3B,&b,camera);
+		a3A(0)=lines[i][0];
+		a3A(1)=lines[i][1];
+		a3A(2)=lines[i][2];
+		a3A(3)=lines[i][3];
+		a3B(0)=lines[i][4];
+		a3B(1)=lines[i][5];
+		a3B(2)=lines[i][6];
+		a3B(3)=lines[i][7];
+		camera->project(a3A,a);
+		camera->project(a3B,b);
 		cvDrawline(image,a,b,CV_RGB(0,0,0),cam);
   }
 }

Modified: trunk/src/stable/components/rgbdViewer/util3d.h
===================================================================
--- trunk/src/stable/components/rgbdViewer/util3d.h	2013-11-29 11:31:57 UTC (rev 1120)
+++ trunk/src/stable/components/rgbdViewer/util3d.h	2013-11-29 13:17:28 UTC (rev 1121)
@@ -22,13 +22,16 @@
 #ifndef KINECTVIEWER_UTIL3D_H
 #define KINECTVIEWER_UTIL3D_H
 
-#include "cv.h"
+#include <cv.h>
 #include "myprogeo.h"
+#include <eigen3/Eigen/Dense>
+#include <geometry/progeo/Progeo.h>
 
 
 
 
-namespace kinectViewer {
+
+namespace rgbdViewer {
 class util3d {
 	public:
 		util3d(myprogeo* p);
@@ -36,7 +39,7 @@
 		void draw_room(cv::Mat image,int cam, float lines[][8], int n_lines);
 
 	private:
-		int cvDrawline(cv::Mat image,HPoint2D p1, HPoint2D p2, cv::Scalar color,int cam);
+		int cvDrawline(cv::Mat image,Eigen::Vector3d p1, Eigen::Vector3d p2, cv::Scalar color,int cam);
 		myprogeo* mypro;
 	};
 }



More information about the Jderobot-admin mailing list