[Jderobot-admin] jderobot-r1012 - in trunk/src/stable: components/basic_component components/cameraserver components/cameraview components/gazeboserver components/gazeboserver/pluginsGazebo/kinect components/introrob components/kinectViewer components/opencvdemo components/openniServer components/recorder components/replayer components/rgbdCalibrator components/rgbdManualCalibrator libs libs/geometry/math libs/geometry/progeo libs/parallelIce libs/visionlib libs/visionlib/colorspaces

rocapal en jderobot.org rocapal en jderobot.org
Lun Oct 7 15:38:13 CEST 2013


Author: rocapal
Date: 2013-10-07 15:37:13 +0200 (Mon, 07 Oct 2013)
New Revision: 1012

Added:
   trunk/src/stable/libs/visionlib/colorspaces/
Removed:
   trunk/src/stable/libs/colorspaces/
Modified:
   trunk/src/stable/components/basic_component/API.h
   trunk/src/stable/components/basic_component/control.h
   trunk/src/stable/components/basic_component/gui.h
   trunk/src/stable/components/cameraserver/cameraserver.cpp
   trunk/src/stable/components/cameraview/cameraview.cfg
   trunk/src/stable/components/cameraview/cameraview.cpp
   trunk/src/stable/components/cameraview/viewer.h
   trunk/src/stable/components/gazeboserver/camera_dump.cc
   trunk/src/stable/components/gazeboserver/pluginsGazebo/kinect/kinectPlugin.h
   trunk/src/stable/components/introrob/API.h
   trunk/src/stable/components/introrob/control.h
   trunk/src/stable/components/introrob/gui.h
   trunk/src/stable/components/kinectViewer/kinectViewer.cfg
   trunk/src/stable/components/kinectViewer/kinectViewergui.cpp
   trunk/src/stable/components/kinectViewer/kinectViewergui.h
   trunk/src/stable/components/opencvdemo/opencvdemo.cfg
   trunk/src/stable/components/opencvdemo/opencvdemo.cpp
   trunk/src/stable/components/opencvdemo/viewer.h
   trunk/src/stable/components/openniServer/openniServer.cfg
   trunk/src/stable/components/openniServer/openniServer.cpp
   trunk/src/stable/components/recorder/poolWriteImages.h
   trunk/src/stable/components/recorder/poolWriteLasers.h
   trunk/src/stable/components/recorder/poolWritePointCloud.h
   trunk/src/stable/components/recorder/recorder.cpp
   trunk/src/stable/components/recorder/recordergui.h
   trunk/src/stable/components/replayer/replayer.cpp
   trunk/src/stable/components/rgbdCalibrator/calibration.cpp
   trunk/src/stable/components/rgbdCalibrator/calibration.h
   trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.cpp
   trunk/src/stable/components/rgbdCalibrator/viewer.cpp
   trunk/src/stable/components/rgbdCalibrator/viewer.h
   trunk/src/stable/components/rgbdManualCalibrator/controller.h
   trunk/src/stable/components/rgbdManualCalibrator/rgbdManualCalibrator.cpp
   trunk/src/stable/components/rgbdManualCalibrator/view.h
   trunk/src/stable/libs/geometry/math/Segment3D.cpp
   trunk/src/stable/libs/geometry/progeo/Progeo.cpp
   trunk/src/stable/libs/parallelIce/cameraClient.h
   trunk/src/stable/libs/visionlib/CMakeLists.txt
   trunk/src/stable/libs/visionlib/colorspaces/CMakeLists.txt
   trunk/src/stable/libs/visionlib/linesDetection.h
Log:
#56 changed colorspaces into visionlib


Modified: trunk/src/stable/components/basic_component/API.h
===================================================================
--- trunk/src/stable/components/basic_component/API.h	2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/basic_component/API.h	2013-10-07 13:37:13 UTC (rev 1012)
@@ -33,7 +33,7 @@
 #include <jderobot/laser.h>
 #include <jderobot/encoders.h>
 #include <jderobot/ptencoders.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
 #include <pthread.h>
 
 

Modified: trunk/src/stable/components/basic_component/control.h
===================================================================
--- trunk/src/stable/components/basic_component/control.h	2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/basic_component/control.h	2013-10-07 13:37:13 UTC (rev 1012)
@@ -33,7 +33,7 @@
 #include <jderobot/laser.h>
 #include <jderobot/encoders.h>
 #include <jderobot/ptencoders.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
 #include <pthread.h>
 #include "API.h"
 

Modified: trunk/src/stable/components/basic_component/gui.h
===================================================================
--- trunk/src/stable/components/basic_component/gui.h	2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/basic_component/gui.h	2013-10-07 13:37:13 UTC (rev 1012)
@@ -22,7 +22,7 @@
 #ifndef BASIC_COMPONENT_GUI_H
 #define BASIC_COMPONENT_GUI_H
 
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
 #include<gtk-2.0/gtk/gtk.h>
 #include<gtk-2.0/gdk/gdk.h>
 #include<gtkmm-2.4/gtkmm.h>
@@ -77,4 +77,4 @@
 
     }; //class
 }//namespace
-#endif //BASIC_COMPONENT_GUI_H
\ No newline at end of file
+#endif //BASIC_COMPONENT_GUI_H

Modified: trunk/src/stable/components/cameraserver/cameraserver.cpp
===================================================================
--- trunk/src/stable/components/cameraserver/cameraserver.cpp	2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/cameraserver/cameraserver.cpp	2013-10-07 13:37:13 UTC (rev 1012)
@@ -26,7 +26,7 @@
 
 #include <jderobot/camera.h>
 #include <jderobot/image.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
 
 
 //Opencv

Modified: trunk/src/stable/components/cameraview/cameraview.cfg
===================================================================
--- trunk/src/stable/components/cameraview/cameraview.cfg	2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/cameraview/cameraview.cfg	2013-10-07 13:37:13 UTC (rev 1012)
@@ -1 +1 @@
-Cameraview.Camera.Proxy=cameraB:default -h localhost -p 9999
+Cameraview.Camera.Proxy=cameraB:default -h localhost -p 9998

Modified: trunk/src/stable/components/cameraview/cameraview.cpp
===================================================================
--- trunk/src/stable/components/cameraview/cameraview.cpp	2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/cameraview/cameraview.cpp	2013-10-07 13:37:13 UTC (rev 1012)
@@ -23,7 +23,7 @@
 #include <Ice/Ice.h>
 #include <IceUtil/IceUtil.h>
 #include <jderobot/camera.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
 #include "viewer.h"
 
 

Modified: trunk/src/stable/components/cameraview/viewer.h
===================================================================
--- trunk/src/stable/components/cameraview/viewer.h	2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/cameraview/viewer.h	2013-10-07 13:37:13 UTC (rev 1012)
@@ -27,7 +27,7 @@
 #include <IceUtil/Thread.h>
 #include <IceUtil/Time.h>
 #include <string>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
 
 namespace cameraview{
 

Modified: trunk/src/stable/components/gazeboserver/camera_dump.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/camera_dump.cc	2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/gazeboserver/camera_dump.cc	2013-10-07 13:37:13 UTC (rev 1012)
@@ -13,7 +13,7 @@
 
 #include <jderobot/camera.h>
 
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
 
 #include <iostream>
 

Modified: trunk/src/stable/components/gazeboserver/pluginsGazebo/kinect/kinectPlugin.h
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/kinect/kinectPlugin.h	2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/kinect/kinectPlugin.h	2013-10-07 13:37:13 UTC (rev 1012)
@@ -35,7 +35,7 @@
 #include <IceUtil/IceUtil.h>
 
 
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
 
 #include <jderobot/pointcloud.h>
 #include <jderobot/camera.h>

Modified: trunk/src/stable/components/introrob/API.h
===================================================================
--- trunk/src/stable/components/introrob/API.h	2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/introrob/API.h	2013-10-07 13:37:13 UTC (rev 1012)
@@ -38,7 +38,7 @@
 #include <jderobot/laser.h>
 #include <jderobot/encoders.h>
 #include <jderobot/ptencoders.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
 #include <jderobot/pose3dmotors.h>
 #include <jderobot/pose3dencoders.h>
 #include <pthread.h>

Modified: trunk/src/stable/components/introrob/control.h
===================================================================
--- trunk/src/stable/components/introrob/control.h	2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/introrob/control.h	2013-10-07 13:37:13 UTC (rev 1012)
@@ -33,7 +33,7 @@
 #include <jderobot/laser.h>
 #include <jderobot/encoders.h>
 #include <jderobot/ptencoders.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
 
 #include <pthread.h>
 #include "API.h"

Modified: trunk/src/stable/components/introrob/gui.h
===================================================================
--- trunk/src/stable/components/introrob/gui.h	2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/introrob/gui.h	2013-10-07 13:37:13 UTC (rev 1012)
@@ -37,7 +37,7 @@
 #include <IceUtil/Time.h>
 #include <jderobot/camera.h>
 #include <pthread.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
 #include <gtkmm/drawingarea.h>
 #include <gdkmm/pixbuf.h>
 #include "API.h"

Modified: trunk/src/stable/components/kinectViewer/kinectViewer.cfg
===================================================================
--- trunk/src/stable/components/kinectViewer/kinectViewer.cfg	2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/kinectViewer/kinectViewer.cfg	2013-10-07 13:37:13 UTC (rev 1012)
@@ -1,9 +1,9 @@
 kinectViewer.CameraRGBActive=1
-kinectViewer.CameraRGB.Proxy=cameraA:tcp -h 127.0.0.1 -p 9999
+kinectViewer.CameraRGB.Proxy=cameraA:tcp -h 127.0.0.1 -p 9998
 kinectViewer.CameraDEPTHActive=1
-kinectViewer.CameraDEPTH.Proxy=cameraB:tcp -h 127.0.0.1 -p 9999
+kinectViewer.CameraDEPTH.Proxy=cameraB:tcp -h 127.0.0.1 -p 9998
 kinectViewer.pointCloudActive=1
-kinectViewer.pointCloud.Proxy=pointcloud1:tcp -h 127.0.0.1 -p 9999
+kinectViewer.pointCloud.Proxy=pointcloud1:tcp -h 127.0.0.1 -p 9998
 kinectViewer.Pose3DMotorsActive=0
 kinectViewer.Pose3DMotors.Proxy=Pose3DMotors1:tcp -h 127.0.0.1 -p 9998
 kinectViewer.KinectLedsActive=0

Modified: trunk/src/stable/components/kinectViewer/kinectViewergui.cpp
===================================================================
--- trunk/src/stable/components/kinectViewer/kinectViewergui.cpp	2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/kinectViewer/kinectViewergui.cpp	2013-10-07 13:37:13 UTC (rev 1012)
@@ -23,562 +23,567 @@
 #include <jderobot/pointcloud.h>
 
 namespace kinectViewer {
-	kinectViewergui::kinectViewergui(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) {
+kinectViewergui::kinectViewergui(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) {
 
-		/*Init OpenGL*/
-		if(!Gtk::GL::init_check(NULL, NULL))	{
-			std::cerr << "Couldn't initialize GL\n";
-			std::exit(1);
-		}
-		this->cycle=cycle;
+    /*Init OpenGL*/
+    if(!Gtk::GL::init_check(NULL, NULL))	{
+        std::cerr << "Couldn't initialize GL\n";
+        std::exit(1);
+    }
+    this->cycle=cycle;
 
-	if (rgb)
-		cam_rgb_active=1;
-	else
-		cam_rgb_active=0;
-	if (depth)
-		cam_depth_active=1;
-	else
-		cam_depth_active=0;
-	modesAvalables=0;
-	if (pointCloud){
-		if ((rgb)&&(depth)){
-			reconstructMode=1;
-			modesAvalables=2; //only one mode
-		}
-		else{
-			reconstructMode=1;
-			modesAvalables=1; //both modes
-		}
-	}
-	else{
-		if ((rgb)&&(depth)){
-			reconstructMode=1;
-			modesAvalables=1; //only point cloud mode 
-		}
-		else{
-			reconstructMode=0;
-			modesAvalables=0; // no mode available
-		}
-	}
+    if (rgb)
+        cam_rgb_active=1;
+    else
+        cam_rgb_active=0;
+    if (depth)
+        cam_depth_active=1;
+    else
+        cam_depth_active=0;
+    modesAvalables=0;
+    if (pointCloud) {
+        if ((rgb)&&(depth)) {
+            reconstructMode=1;
+            modesAvalables=2; //only one mode
+        }
+        else {
+            reconstructMode=1;
+            modesAvalables=1; //both modes
+        }
+    }
+    else {
+        if ((rgb)&&(depth)) {
+            reconstructMode=1;
+            modesAvalables=1; //only point cloud mode
+        }
+        else {
+            reconstructMode=0;
+            modesAvalables=0; // no mode available
+        }
+    }
 
-	reconstruct_depth_activate=false;
-	lines_depth_active=false;
-	lines_rgb_active=false;
-	std::cout << "Loading glade\n";
-	refXml = Gnome::Glade::Xml::create("./kinectViewergui.glade");
-	cWidth=width;
-	cHeight=height;
+    reconstruct_depth_activate=false;
+    lines_depth_active=false;
+    lines_rgb_active=false;
+    std::cout << "Loading glade\n";
+    refXml = Gnome::Glade::Xml::create("./kinectViewergui.glade");
+    cWidth=width;
+    cHeight=height;
 
 
-	
 
-	/*Get widgets*/
-	refXml->get_widget("kinectViewer", mainwindow);
-	refXml->get_widget("imageRGB", w_imageRGB);
-	refXml->get_widget("imageDEPTH", w_imageDEPTH);
-	refXml->get_widget("eventboxRGB", w_event_rgb);
-	refXml->get_widget("eventboxDEPTH", w_event_depth);
-	refXml->get_widget("toggle_reconstruct", w_reconstruct);
-	refXml->get_widget("toggle_camera_pos", w_camera_pos);
-	refXml->get_widget("toggle_lines_rgb", w_lines_rgb);
-	refXml->get_widget("toggle_lines_depth", w_lines_depth);
-	refXml->get_widget("toggle_depth", w_toggle_depth);
-	refXml->get_widget("toggle_rgb", w_toggle_rgb);	
-	refXml->get_widget("view_controller",w_view_controller);
-	refXml->get_widget("window_controller",w_window_controller);
-	refXml->get_widget("vbox_reconstruct_selection",w_reconstruct_selection);
-	refXml->get_widget("vbox_reconstruct_mode",w_reconstruct_mode);
-	refXml->get_widget("radio_mode_pointcloud",w_radio_mode_pointcloud);
-	refXml->get_widget("radio_mode_image",w_radio_mode_image);
-	refXml->get_widget("radio_depth",w_radio_depth);
-	refXml->get_widget("radio_rgb",w_radio_rgb);
-	refXml->get_widget("button_clear_lines",w_button_clear_lines);
-	refXml->get_widget("window_gl",w_window_gl);
-	refXml->get_widget("tg_gl",w_tg_gl);
-	refXml->get_widget("vbox_gl",w_vbox_gl);
-		
-	if (!cam_rgb_active){
-		w_toggle_rgb->hide();
-	}
-	if (!cam_depth_active){
-		w_toggle_depth->hide();
-	}
-	
-	w_event_rgb->signal_button_press_event().connect(sigc::mem_fun(this,&kinectViewergui::on_clicked_event_rgb));
-	w_event_depth->signal_button_press_event().connect(sigc::mem_fun(this,&kinectViewergui::on_clicked_event_depth));
-	w_reconstruct->signal_toggled().connect(sigc::mem_fun(this,&kinectViewergui::on_reconstruct_depth));
-	w_camera_pos->signal_toggled().connect(sigc::mem_fun(this,&kinectViewergui::add_camera_position));
-	w_lines_rgb->signal_toggled().connect(sigc::mem_fun(this,&kinectViewergui::on_w_lines_rgb_toggled));
-	w_lines_depth->signal_toggled().connect(sigc::mem_fun(this,&kinectViewergui::on_w_lines_depth_toggled));
-	w_tg_gl->signal_toggled().connect(sigc::mem_fun(this,&kinectViewergui::on_w_tg_gl_toggled));
-	w_view_controller->signal_activate().connect(sigc::mem_fun(this,&kinectViewergui::on_w_view_controller_activate));
-	w_radio_depth->signal_toggled().connect(sigc::mem_fun(this,&kinectViewergui::on_w_radio_depth_activate));
-	w_radio_rgb->signal_toggled().connect(sigc::mem_fun(this,&kinectViewergui::on_w_radio_rgb_activate));
-	w_radio_mode_pointcloud->signal_toggled().connect(sigc::mem_fun(this,&kinectViewergui::on_w_radio_mode_pointcloud_activate));
-	w_radio_mode_image->signal_toggled().connect(sigc::mem_fun(this,&kinectViewergui::on_w_radio_mode_image_activate));
-	w_button_clear_lines->signal_clicked().connect(sigc::mem_fun(this,&kinectViewergui::on_clicked_clear_lines));
 
-	if (modesAvalables==0){
-		w_reconstruct->hide();
-	}
+    /*Get widgets*/
+    refXml->get_widget("kinectViewer", mainwindow);
+    refXml->get_widget("imageRGB", w_imageRGB);
+    refXml->get_widget("imageDEPTH", w_imageDEPTH);
+    refXml->get_widget("eventboxRGB", w_event_rgb);
+    refXml->get_widget("eventboxDEPTH", w_event_depth);
+    refXml->get_widget("toggle_reconstruct", w_reconstruct);
+    refXml->get_widget("toggle_camera_pos", w_camera_pos);
+    refXml->get_widget("toggle_lines_rgb", w_lines_rgb);
+    refXml->get_widget("toggle_lines_depth", w_lines_depth);
+    refXml->get_widget("toggle_depth", w_toggle_depth);
+    refXml->get_widget("toggle_rgb", w_toggle_rgb);
+    refXml->get_widget("view_controller",w_view_controller);
+    refXml->get_widget("window_controller",w_window_controller);
+    refXml->get_widget("vbox_reconstruct_selection",w_reconstruct_selection);
+    refXml->get_widget("vbox_reconstruct_mode",w_reconstruct_mode);
+    refXml->get_widget("radio_mode_pointcloud",w_radio_mode_pointcloud);
+    refXml->get_widget("radio_mode_image",w_radio_mode_image);
+    refXml->get_widget("radio_depth",w_radio_depth);
+    refXml->get_widget("radio_rgb",w_radio_rgb);
+    refXml->get_widget("button_clear_lines",w_button_clear_lines);
+    refXml->get_widget("window_gl",w_window_gl);
+    refXml->get_widget("tg_gl",w_tg_gl);
+    refXml->get_widget("vbox_gl",w_vbox_gl);
 
-		// Mundo OpenGL
+    if (!cam_rgb_active) {
+        w_toggle_rgb->hide();
+    }
+    if (!cam_depth_active) {
+        w_toggle_depth->hide();
+    }
+
+    w_event_rgb->signal_button_press_event().connect(sigc::mem_fun(this,&kinectViewergui::on_clicked_event_rgb));
+    w_event_depth->signal_button_press_event().connect(sigc::mem_fun(this,&kinectViewergui::on_clicked_event_depth));
+    w_reconstruct->signal_toggled().connect(sigc::mem_fun(this,&kinectViewergui::on_reconstruct_depth));
+    w_camera_pos->signal_toggled().connect(sigc::mem_fun(this,&kinectViewergui::add_camera_position));
+    w_lines_rgb->signal_toggled().connect(sigc::mem_fun(this,&kinectViewergui::on_w_lines_rgb_toggled));
+    w_lines_depth->signal_toggled().connect(sigc::mem_fun(this,&kinectViewergui::on_w_lines_depth_toggled));
+    w_tg_gl->signal_toggled().connect(sigc::mem_fun(this,&kinectViewergui::on_w_tg_gl_toggled));
+    w_view_controller->signal_activate().connect(sigc::mem_fun(this,&kinectViewergui::on_w_view_controller_activate));
+    w_radio_depth->signal_toggled().connect(sigc::mem_fun(this,&kinectViewergui::on_w_radio_depth_activate));
+    w_radio_rgb->signal_toggled().connect(sigc::mem_fun(this,&kinectViewergui::on_w_radio_rgb_activate));
+    w_radio_mode_pointcloud->signal_toggled().connect(sigc::mem_fun(this,&kinectViewergui::on_w_radio_mode_pointcloud_activate));
+    w_radio_mode_image->signal_toggled().connect(sigc::mem_fun(this,&kinectViewergui::on_w_radio_mode_image_activate));
+    w_button_clear_lines->signal_clicked().connect(sigc::mem_fun(this,&kinectViewergui::on_clicked_clear_lines));
+
+    if (modesAvalables==0) {
+        w_reconstruct->hide();
+    }
+
+    // Mundo OpenGL
     refXml->get_widget_derived("gl_world",world);
-	world->setCamerasResolution(width,height);
+    world->setCamerasResolution(width,height);
 
-	std::cout << "Creating Progeos Virtual Cameras" << std::endl;
-	mypro= new kinectViewer::myprogeo();
-	mypro->load_cam((char*)path_rgb.c_str(),0,width, height);
+    std::cout << "Creating Progeos Virtual Cameras" << std::endl;
+    mypro= new kinectViewer::myprogeo();
+    mypro->load_cam((char*)path_rgb.c_str(),0,width, height);
 
-	mypro->load_cam((char*)path_ir.c_str(),1,width, height);
-	util = new kinectViewer::util3d(mypro);
+    mypro->load_cam((char*)path_ir.c_str(),1,width, height);
+    util = new kinectViewer::util3d(mypro);
 
-		/*Show window. Note: Set window visibility to false in Glade, otherwise opengl won't work*/
-		world->readFile(path);
-		mainwindow->show();	
+    /*Show window. Note: Set window visibility to false in Glade, otherwise opengl won't work*/
+    world->readFile(path);
+    mainwindow->show();
 
-	}
+}
 
-	kinectViewergui::~kinectViewergui() {
-		//delete this->controller;
-	}
+kinectViewergui::~kinectViewergui() {
+    //delete this->controller;
+}
 
 
-void 
+void
 kinectViewergui::updateAll( cv::Mat imageRGB, cv::Mat imageDEPTH, std::vector<jderobot::RGBPoint> cloud )
 {
-		std::cout << imageRGB.rows << std::endl;
+    std::cout << imageRGB.rows << std::endl;
 
-		cv::Mat distance(imageRGB.rows, imageRGB.cols, CV_32FC1);
-		cv::Mat colorDepth(imageDEPTH.size(),imageDEPTH.type());
-		CvPoint pt1,pt2;
-			if (w_toggle_rgb->get_active()){
-				Glib::RefPtr<Gdk::Pixbuf> imgBuff =  Gdk::Pixbuf::create_from_data((const guint8*) imageRGB.data,Gdk::COLORSPACE_RGB,false,8,imageRGB.cols,imageRGB.rows,imageRGB.step);
-	    		w_imageRGB->clear();
-				/*si queremos pintar las lineas*/
-				if (lines_rgb_active){
-					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))){
-	
-				/*split channels to separate distance from image*/
-				std::vector<cv::Mat> layers;
-				cv::split(imageDEPTH, layers);
+    cv::Mat distance(imageRGB.rows, imageRGB.cols, CV_32FC1);
+    cv::Mat colorDepth(imageDEPTH.size(),imageDEPTH.type());
+    CvPoint pt1,pt2;
+    if (w_toggle_rgb->get_active()) {
+        Glib::RefPtr<Gdk::Pixbuf> imgBuff =  Gdk::Pixbuf::create_from_data((const guint8*) imageRGB.data,Gdk::COLORSPACE_RGB,false,8,imageRGB.cols,imageRGB.rows,imageRGB.step);
+        w_imageRGB->clear();
+        /*si queremos pintar las lineas*/
+        if (lines_rgb_active) {
+            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))) {
 
-				cv::cvtColor(layers[0],colorDepth,CV_GRAY2RGB);
-				/*cv::imshow("color", colorDepth);
-				cv::waitKey(1);*/
+        /*split channels to separate distance from image*/
+        std::vector<cv::Mat> layers;
+        cv::split(imageDEPTH, 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::cvtColor(layers[0],colorDepth,CV_GRAY2RGB);
+        /*cv::imshow("color", colorDepth);
+        cv::waitKey(1);*/
 
-				if (w_toggle_depth->get_active()){
-					cv::Mat localDepth;
-										Glib::RefPtr<Gdk::Pixbuf> imgBuff =  Gdk::Pixbuf::create_from_data((const guint8*) colorDepth.data,Gdk::COLORSPACE_RGB,false,8,colorDepth.cols,colorDepth.rows,colorDepth.step);
-					w_imageDEPTH->clear();
-					if (lines_depth_active){
-						util->draw_room(colorDepth,1, world->lines, world->numlines);
-					}
-					w_imageDEPTH->set(imgBuff);
-				}
-				while (gtkmain.events_pending())
-				      	gtkmain.iteration();
-			}
-			if (reconstruct_depth_activate){
-				if (reconstructMode==0){
-					add_depth_pointsImage(imageRGB, distance);
-				}
-				else
-					add_depth_pointsCloud(cloud);
-			}
-		world->my_expose_event();
-    	while (gtkmain.events_pending())
-      	gtkmain.iteration();
-  	}
+        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);
+            }
+        }
 
-void 
+        if (w_toggle_depth->get_active()) {
+            cv::Mat localDepth;
+            Glib::RefPtr<Gdk::Pixbuf> imgBuff =  Gdk::Pixbuf::create_from_data((const guint8*) colorDepth.data,Gdk::COLORSPACE_RGB,false,8,colorDepth.cols,colorDepth.rows,colorDepth.step);
+            w_imageDEPTH->clear();
+            if (lines_depth_active) {
+                util->draw_room(colorDepth,1, world->lines, world->numlines);
+            }
+            w_imageDEPTH->set(imgBuff);
+        }
+        while (gtkmain.events_pending())
+            gtkmain.iteration();
+    }
+    if (reconstruct_depth_activate) {
+        if (reconstructMode==0) {
+            add_depth_pointsImage(imageRGB, distance);
+        }
+        else
+            add_depth_pointsCloud(cloud);
+    }
+    world->my_expose_event();
+    while (gtkmain.events_pending())
+        gtkmain.iteration();
+}
+
+void
 kinectViewergui::updateRGB( cv::Mat imageRGB)
 {
-		CvPoint pt1,pt2;
-			if (w_toggle_rgb->get_active()){
-				Glib::RefPtr<Gdk::Pixbuf> imgBuff =  Gdk::Pixbuf::create_from_data((const guint8*) imageRGB.data,Gdk::COLORSPACE_RGB,false,8,imageRGB.cols,imageRGB.rows,imageRGB.step);
-	    		w_imageRGB->clear();
-				/*si queremos pintar las lineas*/
-				if (lines_rgb_active){
-					util->draw_room(imageRGB,0, world->lines, world->numlines);
-					
-				}
-	    		w_imageRGB->set(imgBuff);
-	    		displayFrameRate();
-	    		while (gtkmain.events_pending())
-	      		gtkmain.iteration();
-			}
-			if (reconstruct_depth_activate){
-				if (reconstructMode!=0){
-					//add_depth_pointsCloud();
-				}
-			}
-		world->my_expose_event();
-    	while (gtkmain.events_pending())
-      	gtkmain.iteration();
-  	}
+    CvPoint pt1,pt2;
+    if (w_toggle_rgb->get_active()) {
+        Glib::RefPtr<Gdk::Pixbuf> imgBuff =  Gdk::Pixbuf::create_from_data((const guint8*) imageRGB.data,Gdk::COLORSPACE_RGB,false,8,imageRGB.cols,imageRGB.rows,imageRGB.step);
+        w_imageRGB->clear();
+        /*si queremos pintar las lineas*/
+        if (lines_rgb_active) {
+            util->draw_room(imageRGB,0, world->lines, world->numlines);
 
-void 
+        }
+        w_imageRGB->set(imgBuff);
+        displayFrameRate();
+        while (gtkmain.events_pending())
+            gtkmain.iteration();
+    }
+    if (reconstruct_depth_activate) {
+        if (reconstructMode!=0) {
+            //add_depth_pointsCloud();	
+        }
+    }
+    world->my_expose_event();
+    while (gtkmain.events_pending())
+        gtkmain.iteration();
+}
+
+void
 kinectViewergui::updateDEPTH(cv::Mat 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);
-	    		w_imageDEPTH->clear();
-				if (lines_depth_active){
-					util->draw_room(imageDEPTH,1, world->lines, world->numlines);
-					
-				}
-	    		w_imageDEPTH->set(imgBuff);
-	    		displayFrameRate();
-	    		while (gtkmain.events_pending())
-	      		gtkmain.iteration();
-			}
-			if (reconstruct_depth_activate){
-				if (reconstructMode!=0){
-					//add_depth_pointsCloud();
-				}
-			}
-		world->my_expose_event();
-    	while (gtkmain.events_pending())
-      	gtkmain.iteration();
-  	}
+    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);
+        w_imageDEPTH->clear();
+        if (lines_depth_active) {
+            util->draw_room(imageDEPTH,1, world->lines, world->numlines);
 
-void 
+        }
+        w_imageDEPTH->set(imgBuff);
+        displayFrameRate();
+        while (gtkmain.events_pending())
+            gtkmain.iteration();
+    }
+    if (reconstruct_depth_activate) {
+        if (reconstructMode!=0) {
+            //add_depth_pointsCloud();
+        }
+    }
+    world->my_expose_event();
+    while (gtkmain.events_pending())
+        gtkmain.iteration();
+}
+
+void
 kinectViewergui::updatePointCloud(std::vector<jderobot::RGBPoint> cloud )
 {
-	displayFrameRate();
-	    		while (gtkmain.events_pending())
-	      		gtkmain.iteration();
-			if (reconstruct_depth_activate){
-				if (reconstructMode==1){
-					add_depth_pointsCloud(cloud);
-				}
-			}
-		world->my_expose_event();
-    	while (gtkmain.events_pending())
-      	gtkmain.iteration();
-  	}
+    displayFrameRate();
+    while (gtkmain.events_pending())
+        gtkmain.iteration();
+    if (reconstruct_depth_activate) {
+        if (reconstructMode==1) {
+            add_depth_pointsCloud(cloud);
+        }
+    }
+    world->my_expose_event();
+    while (gtkmain.events_pending())
+        gtkmain.iteration();
+}
 
 
-	bool kinectViewergui::isClosed(){
-		return false;
-	}
+bool kinectViewergui::isClosed() {
+    return false;
+}
 
-	  bool kinectViewergui::isVisible() {
+bool kinectViewergui::isVisible() {
     return mainwindow->is_visible();
-  }
+}
 
 void kinectViewergui::displayFrameRate()
 {
-	double diff;
-	IceUtil::Time diffT;
+    double diff;
+    IceUtil::Time diffT;
 
-	currentFrameTime = IceUtil::Time::now();
-	diff = (currentFrameTime - oldFrameTime).toMilliSecondsDouble();
-	if (diff < 1000.0)
-		frameCount++;
-	else{
-		oldFrameTime = currentFrameTime;
-		fps = frameCount*1000.0/diff;
-		frameCount=0;
-		// Display the frame rate
-		std::stringstream fpsString;
-		fpsString << "fps = " << int(fps);
-		//fpslabel->set_label(fpsString.str());
-		}
-	}
+    currentFrameTime = IceUtil::Time::now();
+    diff = (currentFrameTime - oldFrameTime).toMilliSecondsDouble();
+    if (diff < 1000.0)
+        frameCount++;
+    else {
+        oldFrameTime = currentFrameTime;
+        fps = frameCount*1000.0/diff;
+        frameCount=0;
+        // Display the frame rate
+        std::stringstream fpsString;
+        fpsString << "fps = " << int(fps);
+        //fpslabel->set_label(fpsString.str());
+    }
+}
 
-bool kinectViewergui::on_clicked_event_rgb(GdkEventButton* event){
-	int x,y;
-	float xp,yp,zp,camx,camy,camz;
-	float xu,yu,zu; 
-	float k;
-		
-	gdk_window_at_pointer(&x,&y);
-	std::cout << x << ", " << y << std::endl;
-	mypro->mybackproject(x, y, &xp, &yp, &zp, &camx, &camy, &camz,0);
-	xu=(xp-camx)/sqrt((xp-camx)*(xp-camx) + (yp-camy)*(yp-camy) + (zp-camz)*(zp-camz));
-	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));
+bool kinectViewergui::on_clicked_event_rgb(GdkEventButton* event) {
+    int x,y;
+    float xp,yp,zp,camx,camy,camz;
+    float xu,yu,zu;
+    float k;
 
-	k= 5000;
-	world->add_line(camx+k*xu,camy+k*yu,camz+k*zu,camx,camy,camz);
-	return true;
+    gdk_window_at_pointer(&x,&y);
+    std::cout << x << ", " << y << std::endl;
+    mypro->mybackproject(x, y, &xp, &yp, &zp, &camx, &camy, &camz,0);
+    xu=(xp-camx)/sqrt((xp-camx)*(xp-camx) + (yp-camy)*(yp-camy) + (zp-camz)*(zp-camz));
+    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);
+    return true;
 }
 
 
-bool kinectViewergui::on_clicked_event_depth(GdkEventButton* event){
-	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);
-	xu=(xp-camx)/sqrt((xp-camx)*(xp-camx) + (yp-camy)*(yp-camy) + (zp-camz)*(zp-camz));
-	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));
+bool kinectViewergui::on_clicked_event_depth(GdkEventButton* event) {
+    int x,y;
+    float xp,yp,zp,camx,camy,camz;
+    float xu,yu,zu;
+    float k;
 
-	k= 5000;
-	world->add_line(camx+k*xu,camy+k*yu,camz+k*zu,camx,camy,camz);
-	
-	world->add_line(k*xu + camx,k*yu + camy,k*zu + camz,camx,camy,camz);
-	return true;
+    gdk_window_at_pointer(&x,&y);
+    mypro->mybackproject(x, y, &xp, &yp, &zp, &camx, &camy, &camz,1);
+    xu=(xp-camx)/sqrt((xp-camx)*(xp-camx) + (yp-camy)*(yp-camy) + (zp-camz)*(zp-camz));
+    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);
+
+    world->add_line(k*xu + camx,k*yu + camy,k*zu + camz,camx,camy,camz);
+    return true;
 }
 
-void kinectViewergui::add_cameras_position(){
-	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);
-	xu=(xp-camx)/sqrt((xp-camx)*(xp-camx) + (yp-camy)*(yp-camy) + (zp-camz)*(zp-camz));
-	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));
+void kinectViewergui::add_cameras_position() {
+    int x,y;
+    float xp,yp,zp,camx,camy,camz;
+    float xu,yu,zu;
+    float k;
 
-	k= 500;
-	world->add_line(camx+k*xu,camy+k*yu,camz+k*zu,camx,camy,camz);
-	world->add_line(1905,800,1240,camx,camy,camz);
+    gdk_window_at_pointer(&x,&y);
+    mypro->mybackproject(x, y, &xp, &yp, &zp, &camx, &camy, &camz,0);
+    xu=(xp-camx)/sqrt((xp-camx)*(xp-camx) + (yp-camy)*(yp-camy) + (zp-camz)*(zp-camz));
+    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= 500;
+    world->add_line(camx+k*xu,camy+k*yu,camz+k*zu,camx,camy,camz);
+    world->add_line(1905,800,1240,camx,camy,camz);
 }
 
-void 
-kinectViewergui::on_reconstruct_depth(){
-	if (w_reconstruct->get_active()){
-		reconstruct_depth_activate=true;
-		world->draw_kinect_points=true;
-		w_reconstruct_selection->show();
-		if (modesAvalables==2)
-			w_reconstruct_mode->show();
-	}
-	else{
-		reconstruct_depth_activate=false;
-		world->draw_kinect_points=false;
-		w_reconstruct_selection->hide();
-	}
+void
+kinectViewergui::on_reconstruct_depth() {
+    if (w_reconstruct->get_active()) {
+        reconstruct_depth_activate=true;
+        world->draw_kinect_points=true;
+        w_reconstruct_selection->show();
+        if (modesAvalables==2)
+            w_reconstruct_mode->show();
+    }
+    else {
+        reconstruct_depth_activate=false;
+        world->draw_kinect_points=false;
+        w_reconstruct_selection->hide();
+    }
 }
 
-void 
-kinectViewergui::add_depth_pointsImage(cv::Mat imageRGB, cv::Mat distance){
-	float d;
-		//std::cout << "point image" << std::endl;
+void
+kinectViewergui::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++){
-			d=distance.at<float>(yIm,xIm);
-			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->clear_points();
+    //std::cout << "inicio reconstrucción" << std::endl;
+    for (int xIm=0; xIm< cWidth; xIm++) {
+        for (int yIm=0; yIm<cHeight ; yIm++) {
+            d=distance.at<float>(yIm,xIm);
+            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(xIm, yIm, &xp, &yp, &zp, &camx, &camy, &camz,0);
+
 			
-				mypro->mybackproject(xIm, yIm, &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;
+                //vector unitario	
+                float modulo;
 
-				Fx= d*fx + camx;
-				Fy= d*fy + camy;
-				Fz= d*fz + camz;
+                modulo = sqrt(1/(((camx-xp)*(camx-xp))+((camy-yp)*(camy-yp))+((camz-zp)*(camz-zp))));
+                mypro->mygetcamerafoa(&c1x, &c1y, &c1z, 0);
 
-				/* calculamos el punto real */
-				t = (-(fx*camx) + (fx*Fx) - (fy*camy) + (fy*Fy) - (fz*camz) + (fz*Fz))/((fx*ux) + (fy*uy) + (fz*uz));
 
-			
+                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;
 
-				/*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;
-				//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]);
+                Fx= d*fx + camx;
+                Fy= d*fy + camy;
+                Fz= d*fz + camz;
 
-				//world->add_line(distance*ux + camx,distance*uy+ camy,distance*uz + camz,camx,camy,camz);
-				}
-			}
-		}
+                /* calculamos el punto real */
+                t = (-(fx*camx) + (fx*Fx) - (fy*camy) + (fy*Fy) - (fz*camz) + (fz*Fz))/((fx*ux) + (fy*uy) + (fz*uz));
 
-	//std::cout << "fin reconstrucción" << std::endl;
+
+
+                /*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]);
+
+                //world->add_line(distance*ux + camx,distance*uy+ camy,distance*uz + camz,camx,camy,camz);
+            }
+        }
+    }
+
+    //std::cout << "fin reconstrucción" << std::endl;
 }
 
-void 
-kinectViewergui::add_depth_pointsCloud(std::vector<jderobot::RGBPoint> cloud){
-	world->clear_points();
-	for (std::vector<jderobot::RGBPoint>::iterator it = cloud.begin(); it != cloud.end(); ++it){
-		world->add_kinect_point(it->x,it->y,it->z,(int)it->r,(int)it->g,(int)it->b);
-	}
+void
+kinectViewergui::add_depth_pointsCloud(std::vector<jderobot::RGBPoint> cloud) {
+    world->clear_points();
+    for (std::vector<jderobot::RGBPoint>::iterator it = cloud.begin(); it != cloud.end(); ++it) {
+        world->add_kinect_point(it->x,it->y,it->z,(int)it->r,(int)it->g,(int)it->b);
+    }
 }
 
-void 
-kinectViewergui::add_camera_position(){
-	float c1x, c1y, c1z, c2x, c2y, c2z, c3x, c3y, c3z, c4x, c4y,c4z;
-	float camx, camy, camz;
-	float w,h;
-	float modulo,distance;
-	
+void
+kinectViewergui::add_camera_position() {
+    float c1x, c1y, c1z, c2x, c2y, c2z, c3x, c3y, c3z, c4x, c4y,c4z;
+    float camx, camy, camz;
+    float w,h;
+    float modulo,distance;
 
-	if (w_camera_pos->get_active()){
-		distance=300;
-		mypro->mygetcamerasize(&w,&h,1);
-		mypro->mybackproject(0,0,&c1x,&c1y,&c1z,&camx, &camy, &camz,1);
-		mypro->mybackproject(0,cWidth,&c2x,&c2y,&c2z,&camx, &camy, &camz,1);
-		mypro->mybackproject(cHeight,0,&c3x,&c3y,&c3z,&camx, &camy, &camz,1);
-		mypro->mybackproject(cHeight,cWidth,&c4x,&c4y,&c4z,&camx, &camy, &camz,1);
-		
-		modulo = 	sqrt(1/(((camx-c1x)*(camx-c1x))+((camy-c1y)*(camy-c1y))+((camz-c1z)*(camz-c1z))));
-		c1x = (c1x-camx)*modulo;
-		c1y = (c1y-camy)*modulo;
-		c1z = (c1z-camz)*modulo;
-	
-		modulo = 	sqrt(1/(((camx-c2x)*(camx-c2x))+((camy-c2y)*(camy-c2y))+((camz-c2z)*(camz-c2z))));
-		c2x = (c2x-camx)*modulo;
-		c2y = (c2y-camy)*modulo;
-		c2z = (c2z-camz)*modulo;
-	
-		modulo = 	sqrt(1/(((camx-c3x)*(camx-c3x))+((camy-c3y)*(camy-c3y))+((camz-c3z)*(camz-c3z))));
-		c3x = (c3x-camx)*modulo;
-		c3y = (c3y-camy)*modulo;
-		c3z = (c3z-camz)*modulo;
-	
-		modulo = 	sqrt(1/(((camx-c4x)*(camx-c4x))+((camy-c4y)*(camy-c4y))+((camz-c4z)*(camz-c4z))));
-		c4x = (c4x-camx)*modulo;
-		c4y = (c4y-camy)*modulo;
-		c4z = (c4z-camz)*modulo;
-	
-		
-	
-		world->add_camera_line(distance*c1x + camx,distance*c1y+ camy,distance*c1z + camz,camx,camy,camz);
-		world->add_camera_line(distance*c2x + camx,distance*c2y+ camy,distance*c2z + camz,camx,camy,camz);
-		world->add_camera_line(distance*c3x + camx,distance*c3y+ camy,distance*c3z + camz,camx,camy,camz);
-		world->add_camera_line(distance*c4x + camx,distance*c4y+ camy,distance*c4z + camz,camx,camy,camz);
-		world->add_camera_line(distance*c1x + camx,distance*c1y+ camy,distance*c1z + camz,distance*c2x + camx,distance*c2y+ camy,distance*c2z + camz);
-		world->add_camera_line(distance*c4x + camx,distance*c4y+ camy,distance*c4z + camz,distance*c2x + camx,distance*c2y+ camy,distance*c2z + camz);
-		world->add_camera_line(distance*c3x + camx,distance*c3y+ camy,distance*c3z + camz,distance*c4x + camx,distance*c4y+ camy,distance*c4z + camz);
-		world->add_camera_line(distance*c1x + camx,distance*c1y+ camy,distance*c1z + camz,distance*c3x + camx,distance*c3y+ camy,distance*c3z + camz);
-	
-		mypro->mygetcamerafoa(&c1x, &c1y, &c1z, 1);
-		modulo = 	sqrt(1/(((camx-c1x)*(camx-c1x))+((camy-c1y)*(camy-c1y))+((camz-c1z)*(camz-c1z))));
-		c1x = (c1x-camx)*modulo;
-		c1y = (c1y-camy)*modulo;
-		c1z = (c1z-camz)*modulo;
-		distance=distance*3;
-		world->add_camera_line(distance*c1x + camx,distance*c1y+ camy,distance*c1z + camz,camx,camy,camz);
-	}
-	else{
-		world->clear_camera_lines();
-	}
 
+    if (w_camera_pos->get_active()) {
+        distance=300;
+        mypro->mygetcamerasize(&w,&h,1);
+        mypro->mybackproject(0,0,&c1x,&c1y,&c1z,&camx, &camy, &camz,1);
+        mypro->mybackproject(0,cWidth,&c2x,&c2y,&c2z,&camx, &camy, &camz,1);
+        mypro->mybackproject(cHeight,0,&c3x,&c3y,&c3z,&camx, &camy, &camz,1);
+        mypro->mybackproject(cHeight,cWidth,&c4x,&c4y,&c4z,&camx, &camy, &camz,1);
 
-	//mypro->mybackproject(x, y, &xp, &yp, &zp, &camx, &camy, &camz,1);
+        modulo = 	sqrt(1/(((camx-c1x)*(camx-c1x))+((camy-c1y)*(camy-c1y))+((camz-c1z)*(camz-c1z))));
+        c1x = (c1x-camx)*modulo;
+        c1y = (c1y-camy)*modulo;
+        c1z = (c1z-camz)*modulo;
+
+        modulo = 	sqrt(1/(((camx-c2x)*(camx-c2x))+((camy-c2y)*(camy-c2y))+((camz-c2z)*(camz-c2z))));
+        c2x = (c2x-camx)*modulo;
+        c2y = (c2y-camy)*modulo;
+        c2z = (c2z-camz)*modulo;
+
+        modulo = 	sqrt(1/(((camx-c3x)*(camx-c3x))+((camy-c3y)*(camy-c3y))+((camz-c3z)*(camz-c3z))));
+        c3x = (c3x-camx)*modulo;
+        c3y = (c3y-camy)*modulo;
+        c3z = (c3z-camz)*modulo;
+
+        modulo = 	sqrt(1/(((camx-c4x)*(camx-c4x))+((camy-c4y)*(camy-c4y))+((camz-c4z)*(camz-c4z))));
+        c4x = (c4x-camx)*modulo;
+        c4y = (c4y-camy)*modulo;
+        c4z = (c4z-camz)*modulo;
+
+
+
+        world->add_camera_line(distance*c1x + camx,distance*c1y+ camy,distance*c1z + camz,camx,camy,camz);
+        world->add_camera_line(distance*c2x + camx,distance*c2y+ camy,distance*c2z + camz,camx,camy,camz);
+        world->add_camera_line(distance*c3x + camx,distance*c3y+ camy,distance*c3z + camz,camx,camy,camz);
+        world->add_camera_line(distance*c4x + camx,distance*c4y+ camy,distance*c4z + camz,camx,camy,camz);
+        world->add_camera_line(distance*c1x + camx,distance*c1y+ camy,distance*c1z + camz,distance*c2x + camx,distance*c2y+ camy,distance*c2z + camz);
+        world->add_camera_line(distance*c4x + camx,distance*c4y+ camy,distance*c4z + camz,distance*c2x + camx,distance*c2y+ camy,distance*c2z + camz);
+        world->add_camera_line(distance*c3x + camx,distance*c3y+ camy,distance*c3z + camz,distance*c4x + camx,distance*c4y+ camy,distance*c4z + camz);
+        world->add_camera_line(distance*c1x + camx,distance*c1y+ camy,distance*c1z + camz,distance*c3x + camx,distance*c3y+ camy,distance*c3z + camz);
+
+        mypro->mygetcamerafoa(&c1x, &c1y, &c1z, 1);
+        modulo = 	sqrt(1/(((camx-c1x)*(camx-c1x))+((camy-c1y)*(camy-c1y))+((camz-c1z)*(camz-c1z))));
+        c1x = (c1x-camx)*modulo;
+        c1y = (c1y-camy)*modulo;
+        c1z = (c1z-camz)*modulo;
+        distance=distance*3;
+        world->add_camera_line(distance*c1x + camx,distance*c1y+ camy,distance*c1z + camz,camx,camy,camz);
+    }
+    else {
+        world->clear_camera_lines();
+    }
+
+
+    //mypro->mybackproject(x, y, &xp, &yp, &zp, &camx, &camy, &camz,1);
 }
 
-void 
-kinectViewergui::on_w_lines_rgb_toggled(){
-	if (w_lines_rgb->get_active()){
-		lines_rgb_active=true;
-	}
-	else
-		lines_rgb_active=false;
+void
+kinectViewergui::on_w_lines_rgb_toggled() {
+    if (w_lines_rgb->get_active()) {
+        lines_rgb_active=true;
+    }
+    else
+        lines_rgb_active=false;
 }
 
-void 
-kinectViewergui::on_w_lines_depth_toggled(){
-	if (w_lines_depth->get_active()){
-		lines_depth_active=true;
-	}
-	else
-		lines_depth_active=false;
+void
+kinectViewergui::on_w_lines_depth_toggled() {
+    if (w_lines_depth->get_active()) {
+        lines_depth_active=true;
+    }
+    else
+        lines_depth_active=false;
 }
 
 void
-kinectViewergui::on_w_view_controller_activate(){
-	w_window_controller->show();
+kinectViewergui::on_w_view_controller_activate() {
+    w_window_controller->show();
 }
 
 void
-kinectViewergui::on_w_radio_depth_activate(){
-	if (w_radio_depth->get_active())
-		world->draw_kinect_with_color=false;
+kinectViewergui::on_w_radio_depth_activate() {
+    if (w_radio_depth->get_active())
+        world->draw_kinect_with_color=false;
 }
 
 void
-kinectViewergui::on_w_radio_mode_pointcloud_activate(){
-	if (w_radio_mode_pointcloud->get_active())
-		reconstructMode=1;
+kinectViewergui::on_w_radio_mode_pointcloud_activate() {
+    if (w_radio_mode_pointcloud->get_active())
+        reconstructMode=1;
 }
 
 void
-kinectViewergui::on_w_radio_mode_image_activate(){
-	if (w_radio_mode_image->get_active())
-		reconstructMode=0;
+kinectViewergui::on_w_radio_mode_image_activate() {
+    if (w_radio_mode_image->get_active())
+        reconstructMode=0;
 }
 
 void
-kinectViewergui::on_w_radio_rgb_activate(){
-	if (w_radio_rgb->get_active())
-		world->draw_kinect_with_color=true;
+kinectViewergui::on_w_radio_rgb_activate() {
+    if (w_radio_rgb->get_active())
+        world->draw_kinect_with_color=true;
 }
 
 void
-kinectViewergui::on_clicked_clear_lines(){
-	world->clearExtraLines();
+kinectViewergui::on_clicked_clear_lines() {
+    world->clearExtraLines();
 }
 
-void 
-kinectViewergui::on_w_tg_gl_toggled(){
-	if (w_tg_gl->get_active()){
-		w_window_gl->show();
-		w_vbox_gl->show();
-	}
-	else{
-		w_window_gl->hide();
-		w_vbox_gl->hide();
-	}
+void
+kinectViewergui::on_w_tg_gl_toggled() {
+    if (w_tg_gl->get_active()) {
+        w_window_gl->show();
+        w_vbox_gl->show();
+    }
+    else {
+        w_window_gl->hide();
+        w_vbox_gl->hide();
+    }
 }
 
 float
-kinectViewergui::getCycle(){
-	return this->cycle;
+kinectViewergui::getCycle() {
+    return this->cycle;
 }
 
 } // namespace

Modified: trunk/src/stable/components/kinectViewer/kinectViewergui.h
===================================================================
--- trunk/src/stable/components/kinectViewer/kinectViewergui.h	2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/kinectViewer/kinectViewergui.h	2013-10-07 13:37:13 UTC (rev 1012)
@@ -27,7 +27,7 @@
 #include <gtkmm.h>
 #include <libglademm.h>
 #include "drawarea.h"
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
 #include <jderobot/camera.h>
 #include <IceUtil/Thread.h>
 #include <IceUtil/Time.h>

Modified: trunk/src/stable/components/opencvdemo/opencvdemo.cfg
===================================================================
--- trunk/src/stable/components/opencvdemo/opencvdemo.cfg	2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/opencvdemo/opencvdemo.cfg	2013-10-07 13:37:13 UTC (rev 1012)
@@ -1 +1 @@
-Opencvdemo.Camera.Proxy=cameraA:tcp -h 127.0.0.1 -p 9999
+Opencvdemo.Camera.Proxy=cameraA:tcp -h 127.0.0.1 -p 9998

Modified: trunk/src/stable/components/opencvdemo/opencvdemo.cpp
===================================================================
--- trunk/src/stable/components/opencvdemo/opencvdemo.cpp	2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/opencvdemo/opencvdemo.cpp	2013-10-07 13:37:13 UTC (rev 1012)
@@ -22,7 +22,7 @@
 
 #include <Ice/Ice.h>
 #include <jderobot/camera.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
 #include "viewer.h"
 
 int main(int argc, char** argv){

Modified: trunk/src/stable/components/opencvdemo/viewer.h
===================================================================
--- trunk/src/stable/components/opencvdemo/viewer.h	2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/opencvdemo/viewer.h	2013-10-07 13:37:13 UTC (rev 1012)
@@ -25,7 +25,7 @@
 
 #include <gtkmm.h>
 #include <libglademm.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
 
 #define DEGTORAD     (3.14159264 / 180.0)
 

Modified: trunk/src/stable/components/openniServer/openniServer.cfg
===================================================================
--- trunk/src/stable/components/openniServer/openniServer.cfg	2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/openniServer/openniServer.cfg	2013-10-07 13:37:13 UTC (rev 1012)
@@ -20,14 +20,14 @@
 openniServer.CameraRGB.Format=RGB8
 openniServer.CameraRGB.fps=10
 openniServer.CameraRGB.PlayerDetection=0
-openniServer.CameraRGB.Mirror=1
+openniServer.CameraRGB.Mirror=0
 
 #openniServer.calibration=./config/camRGB
 openniServer.CameraDEPTH.Name=cameraB
 openniServer.CameraDEPTH.Format=RGB8
 openniServer.CameraDEPTH.fps=10
 openniServer.CameraDEPTH.PlayerDetection=0
-openniServer.CameraDEPTH.Mirror=1
+openniServer.CameraDEPTH.Mirror=0
 
 
 

Modified: trunk/src/stable/components/openniServer/openniServer.cpp
===================================================================
--- trunk/src/stable/components/openniServer/openniServer.cpp	2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/openniServer/openniServer.cpp	2013-10-07 13:37:13 UTC (rev 1012)
@@ -29,7 +29,7 @@
 #include <jderobot/camera.h>
 #include <jderobot/pose3dmotors.h>
 #include <jderobot/pointcloud.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
 #include <tr1/memory>
 #include <list>
 #include <sstream>

Modified: trunk/src/stable/components/recorder/poolWriteImages.h
===================================================================
--- trunk/src/stable/components/recorder/poolWriteImages.h	2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/recorder/poolWriteImages.h	2013-10-07 13:37:13 UTC (rev 1012)
@@ -30,7 +30,7 @@
 #include <stdio.h>
 #include <time.h>
 #include <jderobot/camera.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
 #include <fstream>
 
 

Modified: trunk/src/stable/components/recorder/poolWriteLasers.h
===================================================================
--- trunk/src/stable/components/recorder/poolWriteLasers.h	2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/recorder/poolWriteLasers.h	2013-10-07 13:37:13 UTC (rev 1012)
@@ -28,7 +28,7 @@
 #include <stdio.h>
 #include <time.h>
 #include <jderobot/laser.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
 #include <fstream>
 
 

Modified: trunk/src/stable/components/recorder/poolWritePointCloud.h
===================================================================
--- trunk/src/stable/components/recorder/poolWritePointCloud.h	2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/recorder/poolWritePointCloud.h	2013-10-07 13:37:13 UTC (rev 1012)
@@ -28,7 +28,7 @@
 #include <stdio.h>
 #include <time.h>
 #include <jderobot/pointcloud.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
 #include <fstream>
 
 namespace recorder {

Modified: trunk/src/stable/components/recorder/recorder.cpp
===================================================================
--- trunk/src/stable/components/recorder/recorder.cpp	2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/recorder/recorder.cpp	2013-10-07 13:37:13 UTC (rev 1012)
@@ -13,7 +13,7 @@
 #include <jderobot/pose3dmotors.h>
 #include <jderobot/pose3dencoders.h>
 #include <jderobot/pointcloud.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
 #include <opencv2/core/core.hpp>
 #include <opencv2/highgui/highgui.hpp>
 #include <opencv2/imgproc/imgproc.hpp>

Modified: trunk/src/stable/components/recorder/recordergui.h
===================================================================
--- trunk/src/stable/components/recorder/recordergui.h	2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/recorder/recordergui.h	2013-10-07 13:37:13 UTC (rev 1012)
@@ -29,7 +29,7 @@
 #include <libglademm.h>
 #include <IceUtil/Thread.h>
 #include <IceUtil/Time.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
 
 namespace recorder {
   class recordergui {

Modified: trunk/src/stable/components/replayer/replayer.cpp
===================================================================
--- trunk/src/stable/components/replayer/replayer.cpp	2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/replayer/replayer.cpp	2013-10-07 13:37:13 UTC (rev 1012)
@@ -34,7 +34,7 @@
 #include <cv.h>
 #include <highgui.h>
 
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
 #include <jderobot/camera.h>
 #include <jderobot/pointcloud.h>
 #include <jderobot/pose3dencoders.h>

Modified: trunk/src/stable/components/rgbdCalibrator/calibration.cpp
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/calibration.cpp	2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/rgbdCalibrator/calibration.cpp	2013-10-07 13:37:13 UTC (rev 1012)
@@ -21,460 +21,575 @@
 
 #include <calibration.h>
 #include <iostream> 
-#include "../../libs/geometry/progeo/Progeo.h"
 #include "../../libs/geometry/math/Point3D.h"
 #include "../../libs/geometry/math/Segment3D.h"
+#include <../../libs/progeo/progeo.h>
 
 #include <gsl/gsl_blas.h>
 #include <gsl/gsl_linalg.h> 
 #include <gsl/gsl_matrix.h>
 #include <gsl/gsl_multifit.h>
 
+
 using namespace cv;
 
 namespace rgbdCalibrator{
 
 
-  // This source code is a modification of camera_calibration.cpp
-  // You can see in OpenCV samples: 
-  //     - samples/cpp/tutorial_code/calib3d/camera_calibration/
+// This source code is a modification of camera_calibration.cpp
+// You can see in OpenCV samples:
+//     - samples/cpp/tutorial_code/calib3d/camera_calibration/
 
 
-  Calibration::Calibration()
-  {
-    initPatternPoints();
-  }
+Calibration::Calibration(): mProgeo(NULL)
+{
+	initPatternPoints();
 
-  Calibration::~Calibration()
-  {}
+}
 
-  // Intrinsics 
-  bool Calibration::runCalibrationAndSave(Size &boardSize, 
-					  float squareSize, 
-					  int flag, 
-					  Size imageSize,
-					  Mat&  cameraMatrix, 
-					  Mat& distCoeffs,
-					  vector<vector<Point2f> > imagePoints )
-  {
-    vector<Mat> rvecs, tvecs;
-    vector<float> reprojErrs;
-    double totalAvgErr = 0;
-    
-    bool ok = runCalibration (boardSize, squareSize, flag, imageSize, 
-			      cameraMatrix, distCoeffs, imagePoints, 
-			      rvecs, tvecs, reprojErrs, totalAvgErr);
+Calibration::~Calibration()
+{
+	delete(mProgeo);
+}
 
-    std::cout << (ok ? "Calibration succeeded" : "Calibration failed")
-	 << ". avg re projection error = "  << totalAvgErr ;
+// Intrinsics
+bool Calibration::runCalibrationAndSave(Size &boardSize,
+		float squareSize,
+		int flag,
+		Size imageSize,
+		Mat&  cameraMatrix,
+		Mat& distCoeffs,
+		vector<vector<Point2f> > imagePoints )
+{
+	vector<Mat> rvecs, tvecs;
+	vector<float> reprojErrs;
+	double totalAvgErr = 0;
 
-    mKMatrix = Mat(cameraMatrix);
+	bool ok = runCalibration (boardSize, squareSize, flag, imageSize,
+			cameraMatrix, distCoeffs, imagePoints,
+			rvecs, tvecs, reprojErrs, totalAvgErr);
 
+	std::cout << (ok ? "Calibration succeeded" : "Calibration failed")
+			 << ". avg re projection error = "  << totalAvgErr ;
 
-    return ok;
-  }
- 
-  bool Calibration::runCalibration(Size &boardSize, 
-				   float squareSize, 
-				   int flag, 
-				   Size& imageSize, 
-				   Mat& cameraMatrix, 
-				   Mat& distCoeffs,
-				   vector<vector<Point2f> > imagePoints, 
-				   vector<Mat>& rvecs, 
-				   vector<Mat>& tvecs,
-				   vector<float>& reprojErrs,  
-				   double& totalAvgErr)
-  {
+	mKMatrix = Mat(cameraMatrix);
 
-    Pattern calibrationPattern = CHESSBOARD;
+	initProgeo();
 
-    cameraMatrix = Mat::eye(3, 3, CV_64F);
-    if( flag & CV_CALIB_FIX_ASPECT_RATIO )
-      cameraMatrix.at<double>(0,0) = 1.0;
+	return ok;
+}
 
-    distCoeffs = Mat::zeros(8, 1, CV_64F);
-    
-    vector<vector<Point3f> > objectPoints(1);
-    calcBoardCornerPositions(boardSize, squareSize, objectPoints[0], calibrationPattern);
-    
-    objectPoints.resize(imagePoints.size(),objectPoints[0]);
-    
-    //Find intrinsic and extrinsic camera parameters
-    double rms = calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix,
-                                 distCoeffs, rvecs, tvecs, flag|CV_CALIB_FIX_K4|CV_CALIB_FIX_K5);
-    
-    std::cout << "Re-projection error reported by calibrateCamera: "<< rms << std::endl;
-    
-    bool ok = checkRange(cameraMatrix) && checkRange(distCoeffs);
-    
-    totalAvgErr = computeReprojectionErrors(objectPoints, imagePoints,
-					    rvecs, tvecs, cameraMatrix, distCoeffs, reprojErrs);
-    
-    return ok;
+bool Calibration::runCalibration(Size &boardSize,
+		float squareSize,
+		int flag,
+		Size& imageSize,
+		Mat& cameraMatrix,
+		Mat& distCoeffs,
+		vector<vector<Point2f> > imagePoints,
+		vector<Mat>& rvecs,
+		vector<Mat>& tvecs,
+		vector<float>& reprojErrs,
+		double& totalAvgErr)
+{
+
+	Pattern calibrationPattern = CHESSBOARD;
+
+	cameraMatrix = Mat::eye(3, 3, CV_64F);
+	if( flag & CV_CALIB_FIX_ASPECT_RATIO )
+		cameraMatrix.at<double>(0,0) = 1.0;
+
+	distCoeffs = Mat::zeros(8, 1, CV_64F);
+
+	vector<vector<Point3f> > objectPoints(1);
+	calcBoardCornerPositions(boardSize, squareSize, objectPoints[0], calibrationPattern);
+
+	objectPoints.resize(imagePoints.size(),objectPoints[0]);
+
+	//Find intrinsic and extrinsic camera parameters
+	double rms = calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix,
+			distCoeffs, rvecs, tvecs, flag|CV_CALIB_FIX_K4|CV_CALIB_FIX_K5);
+
+	std::cout << "Re-projection error reported by calibrateCamera: "<< rms << std::endl;
+
+	bool ok = checkRange(cameraMatrix) && checkRange(distCoeffs);
+
+	totalAvgErr = computeReprojectionErrors(objectPoints, imagePoints,
+			rvecs, tvecs, cameraMatrix, distCoeffs, reprojErrs);
+
+	return ok;
 }
 
 
-  void Calibration::calcBoardCornerPositions(Size boardSize, 
-					     float squareSize, 
-					     vector<Point3f>& corners,  
-					     Pattern patternType /*= Settings::CHESSBOARD*/)
-  {
-    corners.clear();
-    
-    switch(patternType)
-      {
-      case CHESSBOARD:
-      case CIRCLES_GRID:
-	for( int i = 0; i < boardSize.height; ++i )
-	  for( int j = 0; j < boardSize.width; ++j )
-	    corners.push_back(Point3f(float( j*squareSize ), float( i*squareSize ), 0));
-	break;
-	
-      case ASYMMETRIC_CIRCLES_GRID:
-	for( int i = 0; i < boardSize.height; i++ )
-	  for( int j = 0; j < boardSize.width; j++ )
-	    corners.push_back(Point3f(float((2*j + i % 2)*squareSize), float(i*squareSize), 0));
-	break;
-      default:
-	break;
-      }
-  }
+void Calibration::calcBoardCornerPositions(Size boardSize,
+		float squareSize,
+		vector<Point3f>& corners,
+		Pattern patternType /*= Settings::CHESSBOARD*/)
+{
+	corners.clear();
 
+	switch(patternType)
+	{
+	case CHESSBOARD:
+	case CIRCLES_GRID:
+		for( int i = 0; i < boardSize.height; ++i )
+			for( int j = 0; j < boardSize.width; ++j )
+				corners.push_back(Point3f(float( j*squareSize ), float( i*squareSize ), 0));
+		break;
 
+	case ASYMMETRIC_CIRCLES_GRID:
+		for( int i = 0; i < boardSize.height; i++ )
+			for( int j = 0; j < boardSize.width; j++ )
+				corners.push_back(Point3f(float((2*j + i % 2)*squareSize), float(i*squareSize), 0));
+		break;
+	default:
+		break;
+	}
+}
 
-  double Calibration::computeReprojectionErrors( const vector<vector<Point3f> >& objectPoints, 
-						 const vector<vector<Point2f> >& imagePoints, 
-						 const vector<Mat>& rvecs, 
-						 const vector<Mat>& tvecs, 
-						 const Mat& cameraMatrix, 
-						 const Mat& distCoeffs, 
-						 vector<float>& perViewErrors)
-  {
-    vector<Point2f> imagePoints2;
-    int i, totalPoints = 0;
-    double totalErr = 0, err;
-    perViewErrors.resize(objectPoints.size());
-    
-    for( i = 0; i < (int)objectPoints.size(); ++i )
-      {
-        projectPoints( Mat(objectPoints[i]), rvecs[i], tvecs[i], cameraMatrix,
-                       distCoeffs, imagePoints2);
-        err = norm(Mat(imagePoints[i]), Mat(imagePoints2), CV_L2);
-	
-        int n = (int)objectPoints[i].size();
-        perViewErrors[i] = (float) std::sqrt(err*err/n);
-        totalErr        += err*err;
-        totalPoints     += n;
-      }
-    
-    return std::sqrt(totalErr/totalPoints);
-  }
 
 
+double Calibration::computeReprojectionErrors( const vector<vector<Point3f> >& objectPoints,
+		const vector<vector<Point2f> >& imagePoints,
+		const vector<Mat>& rvecs,
+		const vector<Mat>& tvecs,
+		const Mat& cameraMatrix,
+		const Mat& distCoeffs,
+		vector<float>& perViewErrors)
+{
+	vector<Point2f> imagePoints2;
+	int i, totalPoints = 0;
+	double totalErr = 0, err;
+	perViewErrors.resize(objectPoints.size());
 
-  void
-  Calibration::BackProjectWithDepth (const Eigen::Vector3d pixel,
-				     const colorspaces::Image depthData,
-				     Eigen::Vector4d& res3D)
-  {
+	for( i = 0; i < (int)objectPoints.size(); ++i )
+	{
+		projectPoints( Mat(objectPoints[i]), rvecs[i], tvecs[i], cameraMatrix,
+				distCoeffs, imagePoints2);
+		err = norm(Mat(imagePoints[i]), Mat(imagePoints2), CV_L2);
 
+		int n = (int)objectPoints[i].size();
+		perViewErrors[i] = (float) std::sqrt(err*err/n);
+		totalErr        += err*err;
+		totalPoints     += n;
+	}
 
-    Eigen::Vector4d posCamera;
-    posCamera(0) = double(0.0);
-    posCamera(1) = double(0.0);
-    posCamera(2) = double(0.0);
+	return std::sqrt(totalErr/totalPoints);
+}
 
+void progeo_old_test ()
+{
+	TPinHoleCamera myCamA;
+	xmlReader(&myCamA, "./cameraB.xml");
+	display_camerainfo(myCamA);
 
-    Eigen::Matrix3d K;
-    K(0,0) = mKMatrix.at<double>(0,0);
-    K(0,1) = mKMatrix.at<double>(0,1);
-    K(0,2) = mKMatrix.at<double>(0,2);
+	update_camera_matrix(&myCamA);
+	display_camerainfo(myCamA);
 
-    K(1,0) = mKMatrix.at<double>(1,0);
-    K(1,1) = mKMatrix.at<double>(1,1);
-    K(1,2) = mKMatrix.at<double>(1,2);
+	HPoint2D pixel;
+	pixel.x =224.;
+	pixel.y =18.;
+	pixel.h = 1.;
 
-    K(2,0) = mKMatrix.at<double>(2,0);
-    K(2,1) = mKMatrix.at<double>(2,1);
-    K(2,2) = mKMatrix.at<double>(2,2);
+	std::cout << pixel.x << " , " << pixel.y << std::endl;
 
-    Eigen::Matrix4d RT; 
-    RT(0,0) = double(1.);
-    RT(0,1) = double(0.);
-    RT(0,2) = double(0.);
-    RT(0,3) = double(0.);
+	HPoint3D p3D;
+	backproject(&p3D, pixel, myCamA);
 
-    RT(1,0) = double(0.);
-    RT(1,1) = double(1.);
-    RT(1,2) = double(0.);
-    RT(1,3) = double(0.);
+	std::cout << p3D.X << " , " << p3D.Y << " , " << p3D.Z << std::endl;
 
-    RT(2,0) = double(0.);
-    RT(2,1) = double(0.);
-    RT(2,2) = double(1.);
-    RT(2,3) = double(0.);
+}
 
-    RT(3,0) = double(0.);
-    RT(3,1) = double(0.);
-    RT(3,2) = double(0.);
-    RT(3,3) = double(1.);
 
-    float depth = (int)depthData.data[((depthData.cols*(int)pixel(1))+(int)pixel(0))*3+1]<<8 | (int)depthData.data[((depthData.cols*(int)pixel(1))+(int)pixel(0))*3+2];
+void Calibration::initProgeo()
+{
 
-    Progeo::Progeo* progeo = new Progeo::Progeo(posCamera, K, RT, 320, 240);
-    //progeo->display_camerainfo(); 
+	Eigen::Vector4d posCamera;
+	posCamera(0) = double(0.0);
+	posCamera(1) = double(0.0);
+	posCamera(2) = double(0.0);
+	posCamera(3) = double(1.0);
 
-    //std::cout << "Pixel (opticas): " << pixel << std::endl;
-   
-    // Optical coordinates
-    Eigen::Vector3d graphic;
-    graphic(0) = pixel(1);
-    graphic(1) = 240 - 1 - pixel(0);
-    graphic(2) = 1.0;
 
-    //std::cout << "Graphic (progeo): " << graphic << std::endl;
+	Eigen::Matrix3d K;
+	K(0,0) = mKMatrix.at<double>(0,0);
+	K(0,1) = mKMatrix.at<double>(0,1);
+	K(0,2) = mKMatrix.at<double>(0,2);
 
-    Eigen::Vector4d p3D;
-    progeo->backproject(graphic, p3D); 
+	K(1,0) = mKMatrix.at<double>(1,0);
+	K(1,1) = mKMatrix.at<double>(1,1);
+	K(1,2) = mKMatrix.at<double>(1,2);
 
-    //std::cout << "P3D: " << p3D << std::endl;
+	K(2,0) = mKMatrix.at<double>(2,0);
+	K(2,1) = mKMatrix.at<double>(2,1);
+	K(2,2) = mKMatrix.at<double>(2,2);
 
-    Point3D *pStart = new Point3D(0.0,0.0,0.0);
-    Point3D *pEnd = new Point3D(p3D);
+	Eigen::Matrix4d RT;
+	RT(0,0) = double(1.);
+	RT(0,1) = double(0.);
+	RT(0,2) = double(0.);
+	RT(0,3) = double(0.);
 
-    Segment3D *segment = new Segment3D(*pStart,*pEnd);
+	RT(1,0) = double(0.);
+	RT(1,1) = double(1.);
+	RT(1,2) = double(0.);
+	RT(1,3) = double(0.);
 
-    //std::cout << "Depth: " << depth << std::endl;
+	RT(2,0) = double(0.);
+	RT(2,1) = double(0.);
+	RT(2,2) = double(1.);
+	RT(2,3) = double(0.);
 
-    Point3D *nP3D = segment->getPointByZ(depth);    
+	RT(3,0) = double(0.);
+	RT(3,1) = double(0.);
+	RT(3,2) = double(0.);
+	RT(3,3) = double(1.);
 
-    res3D = nP3D->getPoint();
+	mProgeo = new Progeo::Progeo(posCamera, K, RT, 320, 240);
+	mProgeo->updateRTMatrix();
+	mProgeo->displayCameraInfo();
+	mProgeo->saveToFile("./CameraA.xml");
 
-    //std::cout << res3D << std::endl;
+	Eigen::Vector3d pixel(224.0, 18., 1.0);
+	Eigen::Vector4d p3d;
+	mProgeo->backproject(pixel,p3d);
+	std::cout << "Pixel: " << pixel << std::endl;
+	std::cout << "P3d: " << p3d << std::endl;
 
-    /*
-    std::cout << "-------------" << std::endl;
-    p3D(0) = 0.;
-    p3D(1) = 0.;
-    p3D(2) = 900.;
-    p3D(3)= 1.;
-    std::cout << p3D << std::endl;
 
-    progeo->project(p3D, optical);
-    std::cout << optical << std::endl;
+	progeo_old_test();
+}
 
-    std::cout << "-------------" << std::endl;
-    optical(0) = 0.;
-    optical(1) = 0.;
-    optical(2) = 1.;
 
-    std::cout << optical << std::endl;
-    progeo->backproject(optical, p3D);
-    std::cout << p3D << std::endl;
-    */
+void
+Calibration::BackProjectWithDepth (const Eigen::Vector3d pixel,
+		const colorspaces::Image depthData,
+		Eigen::Vector4d& res3D)
+{
 
-    delete(segment);
-    delete(pStart);
-    delete(pEnd);
-    delete(nP3D);
-    delete(progeo);
+	float depth = (int)depthData.data[((depthData.cols*(int)pixel(1))+(int)pixel(0))*3+1]<<8 | (int)depthData.data[((depthData.cols*(int)pixel(1))+(int)pixel(0))*3+2];
 
-  }
 
-  void Calibration::initPatternPoints()
-  {
-    // Down line, right to left
-    mPatternPoints.push_back(Eigen::Vector4d (120.,0.,0.,1.));
-    mPatternPoints.push_back(Eigen::Vector4d (60.0,0.,0.,1.));
-    mPatternPoints.push_back(Eigen::Vector4d (0.,0.,0.,1.));    
-    mPatternPoints.push_back(Eigen::Vector4d (0.,0.,60.,1.));
-    mPatternPoints.push_back(Eigen::Vector4d (0.,0.,120.,1.));
+	//std::cout << "Pixel (opticas): " << pixel << std::endl;
 
-    // Up line, right to left
-    mPatternPoints.push_back(Eigen::Vector4d (120.,120.,0.,1.));
-    mPatternPoints.push_back(Eigen::Vector4d (60.,120.,0.,1.));
-    mPatternPoints.push_back(Eigen::Vector4d (0.,120.,0.,1.));
-    mPatternPoints.push_back(Eigen::Vector4d (0.,120.,60.,1.));
-    mPatternPoints.push_back(Eigen::Vector4d (0.,120.,120.,1.));
+	// Optical coordinates
+	Eigen::Vector3d graphic;
+	graphic(0) = pixel(1);
+	graphic(1) = 240 - 1 - pixel(0);
+	graphic(2) = 1.0;
 
-    // Middle line, right to left
-    mPatternPoints.push_back(Eigen::Vector4d (120.,60.,0.,1.));
-    mPatternPoints.push_back(Eigen::Vector4d (0.,60.,0.,1.));
-    mPatternPoints.push_back(Eigen::Vector4d (0.,60.,120.,1.));
-    
-    
-  }
+	//std::cout << "Graphic (progeo): " << graphic << std::endl;
 
-  bool Calibration::addPatternPixel (Eigen::Vector3d pixel, const colorspaces::Image depthData)
-  {
-    mPixelPoints.push_back(pixel);
-    
-    if  (mPixelPoints.size() > mPatternPoints.size())
-      return false;
+	Eigen::Vector4d p3D;
+	mProgeo->backproject(graphic, p3D);
 
-    if (mPixelPoints.size() == mPatternPoints.size())
-    {  
+	//std::cout << "P3D: " << p3D << std::endl;
 
-      std::cout << "\tPixels" << "\t\t" << "Pattern" << "\t\t" << "Camera" << std::endl;
+	Point3D *pStart = new Point3D(0.0,0.0,0.0);
+	Point3D *pEnd = new Point3D(p3D);
 
-      for (int i=0; i<mPixelPoints.size(); i++)
-      {
-        
-         std::cout << "P" << i << "\t(" 
-		   << mPixelPoints[i](0) << "," 
-		   << mPixelPoints[i](1) << ")  \t";
+	Segment3D *segment = new Segment3D(*pStart,*pEnd);
 
-         std::cout << "(" << mPatternPoints[i](0) << "," 
-		   << mPatternPoints[i](1) << "," 
-		   << mPatternPoints[i](2) << ") \t";
+	//std::cout << "Depth: " << depth << std::endl;
 
-	 Eigen::Vector4d pCamera(0.,0.,1000000.,0.);
+	Point3D *nP3D = segment->getPointByZ(depth);
 
-	 // We use a window of 5x5 pixels to avoid errors of the depth image
-	 for (int x=-2; x<=2; x++) {
+	res3D = nP3D->getPoint();
 
-           for (int y=-2;y<=2; y++) {
-	     Eigen::Vector3d pixel(mPixelPoints[i](0)+x,mPixelPoints[i](1)+y,1.0);
-	     Eigen::Vector4d aux;
-	     BackProjectWithDepth (pixel, depthData, aux);
-	     
-	     if (aux(2) != 0 && aux(2) < pCamera(2)) {
-	       pCamera = aux;
-             }
-           }
-         }
+	delete(segment);
+	delete(pStart);
+	delete(pEnd);
+	delete(nP3D);
 
-	 mCameraPoints.push_back(pCamera);
+}
 
-	 std::cout << "(" << pCamera[0] 
-		   << "," << pCamera[1] 
-		   << "," << pCamera[2] << ")"
-		   << std::endl;
+void Calibration::initPatternPoints()
+{
+	// Down line, right to left
+	mPatternPoints.push_back(Eigen::Vector4d (120.,0.,0.,1.));
+	mPatternPoints.push_back(Eigen::Vector4d (60.0,0.,0.,1.));
+	mPatternPoints.push_back(Eigen::Vector4d (0.,0.,0.,1.));
+	mPatternPoints.push_back(Eigen::Vector4d (0.,0.,60.,1.));
+	mPatternPoints.push_back(Eigen::Vector4d (0.,0.,120.,1.));
 
+	// Up line, right to left
+	mPatternPoints.push_back(Eigen::Vector4d (120.,120.,0.,1.));
+	mPatternPoints.push_back(Eigen::Vector4d (60.,120.,0.,1.));
+	mPatternPoints.push_back(Eigen::Vector4d (0.,120.,0.,1.));
+	mPatternPoints.push_back(Eigen::Vector4d (0.,120.,60.,1.));
+	mPatternPoints.push_back(Eigen::Vector4d (0.,120.,120.,1.));
 
-	
-	 
-      }
+	// Middle line, right to left
+	mPatternPoints.push_back(Eigen::Vector4d (120.,60.,0.,1.));
+	mPatternPoints.push_back(Eigen::Vector4d (0.,60.,0.,1.));
+	mPatternPoints.push_back(Eigen::Vector4d (0.,60.,120.,1.));
 
-      mPairPoints.clear();
-      
-      //Build pairs
-      for (int i = 0; i< mPatternPoints.size(); i++)
-	mPairPoints.push_back(std::make_pair(mPatternPoints[i] ,mCameraPoints[i]));
 
-      
-      for (int i = 0; i< mPatternPoints.size(); i++)
-      {
-	std::cout << "(" << mPairPoints[i].first(0) 
-		  << "," << mPairPoints[i].first(1)
-		  << "," << mPairPoints[i].first(2) << ")  ->  ";
+}
 
-	std::cout << "(" << mPairPoints[i].second(0) 
-		  << "," << mPairPoints[i].second(1)
-		  << "," << mPairPoints[i].second(2) << ")"
-	  	  << std::endl;
-      }
-      
-      LSO();      
+bool Calibration::addPatternPixel (Eigen::Vector3d pixel, const colorspaces::Image depthData)
+{
+	mPixelPoints.push_back(pixel);
 
-      mCameraPoints.clear();
-      mPairPoints.clear();
+	if  (mPixelPoints.size() > mPatternPoints.size())
+		return false;
 
-    }
+	if (mPixelPoints.size() == mPatternPoints.size())
+	{
 
-    return true;
+		std::cout << "\tPixels" << "\t\t" << "Pixel(Image)\tPixel(progeo)\tDistance" << "\t"
+				  << "Backproject"  << "\t\t\t" << "Fran Point" << "\t\t" << "Pattern"
+				  <<std::endl;
 
-  }
+		for (int i=0; i<mPixelPoints.size(); i++)
+		{
 
-  void Calibration::test(Eigen::Vector4d pCamera)
-  {
+			std::cout << "P" << i << "\t("
+					<< mPixelPoints[i](0) << ","
+					<< mPixelPoints[i](1) << ")  \t";
 
-    std::cout << "====== TEST ====== " << std::endl;
+			Eigen::Vector4d pCamera(0.,0.,1000000.,0.);
 
-    std::cout << pCamera << std::endl;
-    
-    Eigen::Vector4d s = mRTsolution * pCamera;
+			Eigen::Vector3d pixelChoosen;
 
-    std::cout << s << std::endl;
+			// We use a window of 5x5 pixels to avoid errors of the depth image
+			for (int x=-2; x<=2; x++) {
 
-  }
+				for (int y=-2;y<=2; y++) {
+					Eigen::Vector3d pixel(mPixelPoints[i](0)+x,mPixelPoints[i](1)+y,1.0);
+					Eigen::Vector4d aux;
+					BackProjectWithDepth (pixel, depthData, aux);
 
-  void Calibration::LSO()
-  {
+					if (aux(2) != 0 && aux(2) < pCamera(2)) {
+						pCamera = aux;
+						pixelChoosen = pixel;
+					}
+				}
+			}
 
-    gsl_matrix *ertm=gsl_matrix_alloc(4,4);
-    double chisq;
-    gsl_matrix *x,*cov;
-    gsl_vector *y,*c;
-    gsl_multifit_linear_workspace * work;
-    
-    x=gsl_matrix_calloc(mPairPoints.size()*3,12);
-    cov=gsl_matrix_alloc(12,12);
-    y=gsl_vector_alloc(mPairPoints.size()*3);
-    c=gsl_vector_alloc(12);
-    work = gsl_multifit_linear_alloc (mPairPoints.size()*3,12);
-    double x1,y1,z1,x2,y2,z2;
-    //preaparamos la matriz de ecuaciones 
+			mCameraPoints.push_back(pCamera);
 
-    for (int i=0; i<mPairPoints.size(); i++){
-      x1=mPairPoints[i].first(0);
-      y1=mPairPoints[i].first(1);
-      z1=mPairPoints[i].first(2);
-      x2=mPairPoints[i].second(0);
-      y2=mPairPoints[i].second(1);
-      z2=mPairPoints[i].second(2);
+			// Pixel Image
+			std::cout << "(" << pixelChoosen[0] << "," << pixelChoosen[1] << ")  \t";
 
-      gsl_matrix_set(x, i*3,0, x2);
-      gsl_matrix_set(x, i*3,1, y2);
-      gsl_matrix_set(x, i*3,2, z2);
-      gsl_matrix_set(x, i*3,3, 1);
-      gsl_vector_set(y,i*3, x1);      
-      
-      gsl_matrix_set(x, i*3+1, 4, x2);
-      gsl_matrix_set(x, i*3+1, 5, y2);
-      gsl_matrix_set(x, i*3+1, 6, z2);
-      gsl_matrix_set(x, i*3+1, 7, 1);
-      gsl_vector_set(y,i*3+1, y1);
-      
-      gsl_matrix_set(x, i*3+2,8, x2);
-      gsl_matrix_set(x, i*3+2,9, y2);
-      gsl_matrix_set(x, i*3+2,10, z2);
-      gsl_matrix_set(x, i*3+2,11, 1);
-      gsl_vector_set(y,i*3+2, z1);      
-    }
+			mProgeo->pixel2optical(pixelChoosen);
 
-    gsl_multifit_linear(x,y,c,cov,&chisq,work);
-    gsl_multifit_linear_free(work);
-    
-    for (int i=0;i<3; i++) {
-      for (int j=0;j<4;j++) {
-        double v=c->data[i*4+j];
-        if(sqrt(v*v)<0.0001) 
-	  v=0;
-        //gsl_matrix_set(ertm,i,j,v);
-	mRTsolution(i,j) = v;
-      }
-    }
+			// Pixel Progeo
+			std::cout << "(" << pixelChoosen[0] << "," << pixelChoosen[1] << ")  \t";
 
-    /*
+			// Distance
+			float depth = (int)depthData.data[((depthData.cols*(int)pixelChoosen(1))+(int)pixelChoosen(0))*3+1]<<8 | (int)depthData.data[((depthData.cols*(int)pixelChoosen(1))+(int)pixelChoosen(0))*3+2];
+			std::cout << depth << "\t\t";
+
+			// Pixel backProject
+			Eigen::Vector4d aux3D;
+			mProgeo->backproject(pixelChoosen, aux3D);
+			std::cout << "(" << aux3D[0] << "," << aux3D[1] << "," << aux3D[2] << ")\t";
+
+
+			// fix bug about projection's progeo
+			Eigen::Vector4d p3D;
+			getRealPoint(pixelChoosen, depthData, p3D);
+			std::cout << "\t(" << p3D[0] << "," << p3D[1] << "," << p3D[2] << ")\t\t";
+
+
+			std::cout << "(" << mPatternPoints[i](0) << ","
+					<< mPatternPoints[i](1) << ","
+					<< mPatternPoints[i](2) << ")";
+
+			std::cout << std::endl;
+		}
+
+		mPairPoints.clear();
+
+		//Build pairs
+		for (int i = 0; i< mPatternPoints.size(); i++)
+			mPairPoints.push_back(std::make_pair(mPatternPoints[i] ,mCameraPoints[i]));
+
+
+		for (int i = 0; i< mPatternPoints.size(); i++)
+		{
+			std::cout << "(" << mPairPoints[i].first(0)
+				  << "," << mPairPoints[i].first(1)
+				  << "," << mPairPoints[i].first(2) << ")  ->  ";
+
+			std::cout << "(" << mPairPoints[i].second(0)
+				  << "," << mPairPoints[i].second(1)
+				  << "," << mPairPoints[i].second(2) << ")";
+
+			std::cout << std::endl;
+		}
+
+		LSO();
+
+		mCameraPoints.clear();
+		mPairPoints.clear();
+
+	}
+
+	return true;
+
+}
+
+
+void Calibration::getRealPoint(const Eigen::Vector3d pixel,
+		const colorspaces::Image depthData,
+		Eigen::Vector4d& res3D)
+{
+
+	float depth = (int)depthData.data[((depthData.cols*(int)pixel(1))+(int)pixel(0))*3+1]<<8 | (int)depthData.data[((depthData.cols*(int)pixel(1))+(int)pixel(0))*3+2];
+
+	if (depth == 0)
+	{
+		res3D(0) = 0.;
+		res3D(1) = 0.;
+		res3D(2) = 0.;
+		res3D(3) = 0.;
+		return;
+	}
+
+	Eigen::Vector4d p3D;
+	mProgeo->backproject(pixel, p3D);
+
+	//std::cout << "-> p3D: " << p3D << std::endl;
+
+	Eigen::Vector4d camPosition = mProgeo->getPosition();
+
+	//std::cout << "-> camPosition: " << camPosition << std::endl;
+
+	float module = sqrt(double(1/(((camPosition(0)-p3D(0))*(camPosition(0)-p3D(0)))+
+			((camPosition(1)-p3D(1))*(camPosition(1)-p3D(1)))+
+			((camPosition(2)-p3D(2))*(camPosition(2)-p3D(2))))));
+
+	//std::cout << "-> module: " << module << std::endl;
+
+	Eigen::Vector4d camFoa = mProgeo->getFoa();
+
+	//std::cout << "-> camFoa: " << camFoa << std::endl;
+
+	float fmod = sqrt(double(1/(((camPosition(0)-camFoa(0))*(camPosition(0)-camFoa(0)))+
+			((camPosition(1)-camFoa(1))*(camPosition(1)-camFoa(1)))+
+			((camPosition(2)-camFoa(2))*(camPosition(2)-camFoa(2))))));
+
+	//std::cout << "-> fmod: " << fmod << std::endl;
+
+	Eigen::Vector4d f;
+	f(0) = (camFoa(0)-camPosition(0)) * fmod;
+	f(1) = (camFoa(1)-camPosition(1)) * fmod;
+	f(2) = (camFoa(2)-camPosition(2)) * fmod;
+
+	//std::cout << "-> f: " << f << std::endl;
+
+	Eigen::Vector4d u;
+	u(0) = (p3D(0)-camPosition(0)) * module;
+	u(1) = (p3D(1)-camPosition(1)) * module;
+	u(2) = (p3D(2)-camPosition(2)) * module;
+
+	//std::cout << "-> u: " << u << std::endl;
+
+	Eigen::Vector4d F;
+	F(0) = depth * f(0) + camPosition(0);
+	F(1) = depth * f(1) + camPosition(1);
+	F(2) = depth * f(2) + camPosition(2);
+
+	//std::cout << "-> F: " << F << std::endl;
+
+	float t = (- (f(0)*camPosition(0)) + (f(0)*F(0)) - (f(1)*camPosition(1)) + (f(1)*F(1)) - (f(2)*camPosition(2)) + (f(2)*F(2)))/
+			((f(0)*u(0)) + (f(1)*u(1)) + (f(2)*u(2)));
+
+	//std::cout << "t=" << t << std::endl;
+
+
+	res3D(0) = t*u(0) + camPosition(0);
+	res3D(1) = t*u(1) + camPosition(1);
+	res3D(2) = t*u(2) + camPosition(2);
+
+}
+
+
+void Calibration::test(Eigen::Vector4d pCamera)
+{
+	std::cout << "====== TEST ====== " << std::endl;
+	std::cout << pCamera << std::endl;
+
+	Eigen::Vector4d s = mRTsolution * pCamera;
+	std::cout << s << std::endl;
+}
+
+void Calibration::LSO()
+{
+
+	gsl_matrix *ertm=gsl_matrix_alloc(4,4);
+	double chisq;
+	gsl_matrix *x,*cov;
+	gsl_vector *y,*c;
+	gsl_multifit_linear_workspace * work;
+
+	x=gsl_matrix_calloc(mPairPoints.size()*3,12);
+	cov=gsl_matrix_alloc(12,12);
+	y=gsl_vector_alloc(mPairPoints.size()*3);
+	c=gsl_vector_alloc(12);
+	work = gsl_multifit_linear_alloc (mPairPoints.size()*3,12);
+	double x1,y1,z1,x2,y2,z2;
+	//preaparamos la matriz de ecuaciones
+
+	for (int i=0; i<mPairPoints.size(); i++){
+		x1=mPairPoints[i].first(0);
+		y1=mPairPoints[i].first(1);
+		z1=mPairPoints[i].first(2);
+		x2=mPairPoints[i].second(0);
+		y2=mPairPoints[i].second(1);
+		z2=mPairPoints[i].second(2);
+
+		gsl_matrix_set(x, i*3,0, x2);
+		gsl_matrix_set(x, i*3,1, y2);
+		gsl_matrix_set(x, i*3,2, z2);
+		gsl_matrix_set(x, i*3,3, 1);
+		gsl_vector_set(y,i*3, x1);
+
+		gsl_matrix_set(x, i*3+1, 4, x2);
+		gsl_matrix_set(x, i*3+1, 5, y2);
+		gsl_matrix_set(x, i*3+1, 6, z2);
+		gsl_matrix_set(x, i*3+1, 7, 1);
+		gsl_vector_set(y,i*3+1, y1);
+
+		gsl_matrix_set(x, i*3+2,8, x2);
+		gsl_matrix_set(x, i*3+2,9, y2);
+		gsl_matrix_set(x, i*3+2,10, z2);
+		gsl_matrix_set(x, i*3+2,11, 1);
+		gsl_vector_set(y,i*3+2, z1);
+	}
+
+	gsl_multifit_linear(x,y,c,cov,&chisq,work);
+	gsl_multifit_linear_free(work);
+
+	for (int i=0;i<3; i++) {
+		for (int j=0;j<4;j++) {
+			double v=c->data[i*4+j];
+			if(sqrt(v*v)<0.0001)
+				v=0;
+			//gsl_matrix_set(ertm,i,j,v);
+			mRTsolution(i,j) = v;
+		}
+	}
+
+	/*
     gsl_matrix_set(ertm,3,0,0);
     gsl_matrix_set(ertm,3,1,0);
     gsl_matrix_set(ertm,3,2,0);
     gsl_matrix_set(ertm,3,3,1);
-    */
+	 */
 
-    mRTsolution(3,0) = 0.;
-    mRTsolution(3,1) = 0.;
-    mRTsolution(3,2) = 0.;
-    mRTsolution(3,3) = 1.;
+	mRTsolution(3,0) = 0.;
+	mRTsolution(3,1) = 0.;
+	mRTsolution(3,2) = 0.;
+	mRTsolution(3,3) = 1.;
 
-    //gsl_matrix_fprintf(stdout,ertm,"%f");
+	//gsl_matrix_fprintf(stdout,ertm,"%f");
 
-    std::cout << mRTsolution << std::endl;
+	std::cout << mRTsolution << std::endl;
 
-    
 
-  }
 
 }
+
+}

Modified: trunk/src/stable/components/rgbdCalibrator/calibration.h
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/calibration.h	2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/rgbdCalibrator/calibration.h	2013-10-07 13:37:13 UTC (rev 1012)
@@ -28,11 +28,12 @@
 #include <opencv2/highgui/highgui.hpp>
 #include <cmath>
 #include <string>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
 #include <jderobot/camera.h>
 #include "../../libs/geometry/math/Point3D.h"
 #include <boost/tuple/tuple.hpp>
 
+#include "../../libs/geometry/progeo/Progeo.h"
 using namespace cv;
 
 namespace rgbdCalibrator
@@ -73,11 +74,19 @@
 
     Eigen::Matrix4d mRTsolution;
 
+    Progeo::Progeo *mProgeo;
+
     std::vector<Eigen::Vector3d> mPixelPoints;
     std::vector<Eigen::Vector4d> mPatternPoints;
     std::vector<Eigen::Vector4d> mCameraPoints;
     std::vector<std::pair<Eigen::Vector4d,Eigen::Vector4d> > mPairPoints;
 
+    void initProgeo();
+
+    void getRealPoint(const Eigen::Vector3d pixel,
+		       const colorspaces::Image depthData,
+		       Eigen::Vector4d& res3D);
+    
     void initPatternPoints();
 
     void LSO();

Modified: trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.cpp
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.cpp	2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.cpp	2013-10-07 13:37:13 UTC (rev 1012)
@@ -23,7 +23,7 @@
 #include <Ice/Ice.h>
 #include <IceUtil/IceUtil.h>
 #include <jderobot/camera.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
 #include "viewer.h"
 #include <cv.h>
 #include <highgui.h>

Modified: trunk/src/stable/components/rgbdCalibrator/viewer.cpp
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/viewer.cpp	2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/rgbdCalibrator/viewer.cpp	2013-10-07 13:37:13 UTC (rev 1012)
@@ -33,510 +33,510 @@
 using namespace cvb;
 
 namespace rgbdCalibrator{
-  
-  
-  const std::string gladepath = std::string(GLADE_DIR) + 
-    std::string("/rgbdCalibrator.glade");
 
-  const std::string pathImage = "./images/";
 
+const std::string gladepath = std::string(GLADE_DIR) +
+		std::string("/rgbdCalibrator.glade");
 
-  Viewer::Viewer() 
-    : gtkmain(0,0),frameCount(0),
-      intrinsicsEnable(0),contPhoto(1),hsvFilter(NULL), mFrameBlob(NULL) {
+const std::string pathImage = "./images/";
 
-    std::cout << "Loading glade\n";
 
-    // ref widgets
-    refXml = Gnome::Glade::Xml::create(gladepath);
-    refXml->get_widget("color_image", gtkimage_color);
-    refXml->get_widget("color_image2", gtkimage_color2);
-    refXml->get_widget("depth_image", gtkimage_depth);
-    refXml->get_widget("hsv_image", gtkimage_hsv);
-    refXml->get_widget("blob_image", gtkimage_blob);
-    refXml->get_widget("mainwindow",mainwindow);
-    refXml->get_widget("fpslabel",fpsLabel);
-    refXml->get_widget("bt_take_photo", btTakePhoto);
-    refXml->get_widget("et_sleep_photo", etSleepPhoto);
-    refXml->get_widget("et_num_photo", etNumPhoto);
-    refXml->get_widget("tv_status", tvStatus);
-    refXml->get_widget("bt_intrinsic_calib", btIntrinsic);
-    refXml->get_widget("eventbox", ebImage);
-    refXml->get_widget("eb_extrinsics", ebImageExtrinsics);
+Viewer::Viewer()
+: gtkmain(0,0),frameCount(0),
+  intrinsicsEnable(0),contPhoto(1),hsvFilter(NULL), mFrameBlob(NULL) {
 
-    // connect signals
-    btTakePhoto->signal_clicked().connect(sigc::mem_fun(this,&Viewer::on_bt_take_photo_clicked));
+	std::cout << "Loading glade\n";
 
-    btIntrinsic->signal_clicked().connect(sigc::mem_fun(this,&Viewer::on_bt_intrinsic));
-    ebImage->signal_button_press_event().connect(sigc::mem_fun(this, &Viewer::on_eventbox_clicked));  
+	// ref widgets
+	refXml = Gnome::Glade::Xml::create(gladepath);
+	refXml->get_widget("color_image", gtkimage_color);
+	refXml->get_widget("color_image2", gtkimage_color2);
+	refXml->get_widget("depth_image", gtkimage_depth);
+	refXml->get_widget("hsv_image", gtkimage_hsv);
+	refXml->get_widget("blob_image", gtkimage_blob);
+	refXml->get_widget("mainwindow",mainwindow);
+	refXml->get_widget("fpslabel",fpsLabel);
+	refXml->get_widget("bt_take_photo", btTakePhoto);
+	refXml->get_widget("et_sleep_photo", etSleepPhoto);
+	refXml->get_widget("et_num_photo", etNumPhoto);
+	refXml->get_widget("tv_status", tvStatus);
+	refXml->get_widget("bt_intrinsic_calib", btIntrinsic);
+	refXml->get_widget("eventbox", ebImage);
+	refXml->get_widget("eb_extrinsics", ebImageExtrinsics);
 
-    ebImageExtrinsics->signal_button_press_event().connect(sigc::mem_fun(this, &Viewer::on_eventbox_extrinsics_clicked)); 
+	// connect signals
+	btTakePhoto->signal_clicked().connect(sigc::mem_fun(this,&Viewer::on_bt_take_photo_clicked));
 
-    // start the timer for calculating the number of frames per second
-    // the images are being displayed at
-    oldFrameTime = IceUtil::Time::now();
+	btIntrinsic->signal_clicked().connect(sigc::mem_fun(this,&Viewer::on_bt_intrinsic));
+	ebImage->signal_button_press_event().connect(sigc::mem_fun(this, &Viewer::on_eventbox_clicked));
 
-    RGB2HSV_init();
-    RGB2HSV_createTable();
+	ebImageExtrinsics->signal_button_press_event().connect(sigc::mem_fun(this, &Viewer::on_eventbox_extrinsics_clicked));
 
-    mCalibration = new Calibration();
+	// start the timer for calculating the number of frames per second
+	// the images are being displayed at
+	oldFrameTime = IceUtil::Time::now();
 
-    pthread_mutex_init(&mutex, NULL);
-    
+	RGB2HSV_init();
+	RGB2HSV_createTable();
 
-  }
-    
+	mCalibration = new Calibration();
 
-  Viewer::~Viewer() 
-  {
-    if(mCalibration)
-      delete(mCalibration);
+	pthread_mutex_init(&mutex, NULL);
 
-    RGB2HSV_destroyTable();
-  }
 
-  bool Viewer::isVisible(){
-    return mainwindow->is_visible();
-    
-  }
+}
 
-  void Viewer::display( const colorspaces::Image& imageColor, const colorspaces::Image& imageDepth )
-  {
-    colorspaces::ImageRGB8 img_rgb8(imageColor);//conversion will happen if needed
-    Glib::RefPtr<Gdk::Pixbuf> imgBuffColor = 
-      Gdk::Pixbuf::create_from_data((const guint8*)img_rgb8.data,
-				    Gdk::COLORSPACE_RGB,
-				    false,
-				    8,
-				    img_rgb8.width,
-				    img_rgb8.height,
-				    img_rgb8.step);
-    
-    gtkimage_color->clear();
-    gtkimage_color->set(imgBuffColor);
 
-    gtkimage_color2->clear();
-    gtkimage_color2->set(imgBuffColor);
+Viewer::~Viewer()
+{
+	if(mCalibration)
+		delete(mCalibration);
 
-    if (intrinsicsEnable)
-      saveImage(imageColor);
-    
-    
-    pthread_mutex_lock(&mutex);
-    imgOrig.create(imageColor.size(), CV_8UC3);
-    imageColor.copyTo(imgOrig);
+	RGB2HSV_destroyTable();
+}
 
-    mImageDepth = imageDepth.clone();
-    pthread_mutex_unlock(&mutex);
-    
-    // Show depth image in color
-    // 3 RGB canals
-    // 0: Image in gray scale
-    // [1,2]: Real data of distance
-    // 1: 8bit MSB
-    // 2: 8bit LSB
+bool Viewer::isVisible(){
+	return mainwindow->is_visible();
 
-    std::vector<cv::Mat> layers;
-    cv::Mat colorDepth(imageDepth.size(),imageDepth.type());
-    cv::split(imageDepth, layers);
+}
 
-    cv::cvtColor(layers[0],colorDepth,CV_GRAY2RGB);
+void Viewer::display( const colorspaces::Image& imageColor, const colorspaces::Image& imageDepth )
+{
+	colorspaces::ImageRGB8 img_rgb8(imageColor);//conversion will happen if needed
+	Glib::RefPtr<Gdk::Pixbuf> imgBuffColor =
+			Gdk::Pixbuf::create_from_data((const guint8*)img_rgb8.data,
+					Gdk::COLORSPACE_RGB,
+					false,
+					8,
+					img_rgb8.width,
+					img_rgb8.height,
+					img_rgb8.step);
 
-    Glib::RefPtr<Gdk::Pixbuf> imgBuffDepth = 
-      Gdk::Pixbuf::create_from_data((const guint8*) colorDepth.data,
-				    Gdk::COLORSPACE_RGB,
-				    false,
-				    8,
-				    imageDepth.width,
-				    imageDepth.height,
-				    imageDepth.step);
+	gtkimage_color->clear();
+	gtkimage_color->set(imgBuffColor);
 
-    gtkimage_depth->clear();
-    gtkimage_depth->set(imgBuffDepth);
+	gtkimage_color2->clear();
+	gtkimage_color2->set(imgBuffColor);
 
-    if (hsvFilter != NULL)
-    {
-      createImageHSV(imageDepth);
+	if (intrinsicsEnable)
+		saveImage(imageColor);
 
-      // Show HSV image
-      Glib::RefPtr<Gdk::Pixbuf> imgBuffHSV = 
-	Gdk::Pixbuf::create_from_data((const guint8*)imgHSV.data,
-				      Gdk::COLORSPACE_RGB,
-				      false,
-				      8,
-				      imgHSV.size().width,
-				      imgHSV.size().height,
-				      imgHSV.step);
-      
-      gtkimage_hsv->clear();
-      gtkimage_hsv->set(imgBuffHSV);
 
-      // Show Blob Image
-      
-      Glib::RefPtr<Gdk::Pixbuf> imgBuffBLOB = 
-	Gdk::Pixbuf::create_from_data(
-				      (guint8*)mFrameBlob->imageData,
-				      Gdk::COLORSPACE_RGB,
-				      false,
-				      mFrameBlob->depth,
-				      mFrameBlob->width,
-				      mFrameBlob->height,
-				      mFrameBlob->widthStep);
+	imgOrig.create(imageColor.size(), CV_8UC3);
+	imageColor.copyTo(imgOrig);
 
-      gtkimage_blob->clear();
-      gtkimage_blob->set(imgBuffBLOB);
-      
-      //cvReleaseImage(&mFrameBlob);
-      
-    }
+	pthread_mutex_lock(&mutex);
+	mImageDepth = imageDepth.clone();
+	pthread_mutex_unlock(&mutex);
 
-    displayFrameRate();
-    mainwindow->resize(1,1);
-    while (gtkmain.events_pending())
-      gtkmain.iteration();
-  }
-    
-  void Viewer::createImageHSV(const colorspaces::Image& imageDepth)
-  {
-    float r,g,b;
+	// Show depth image in color
+	// 3 RGB canals
+	// 0: Image in gray scale
+	// [1,2]: Real data of distance
+	// 1: 8bit MSB
+	// 2: 8bit LSB
 
-    imgHSV.create(imgOrig.size(), CV_8UC1);
-    imgOrig.copyTo(imgHSV);
+	std::vector<cv::Mat> layers;
+	cv::Mat colorDepth(imageDepth.size(),imageDepth.type());
+	cv::split(imageDepth, layers);
 
-    IplImage *threshy=cvCreateImage(imgOrig.size(),8,1);
-    
-    for (int i=0;i< imgHSV.size().width*imgHSV.size().height; i++)
-    {
-      r = (float)(unsigned int)(unsigned char) imgOrig.data[i*3];
-      g = (float)(unsigned int)(unsigned char) imgOrig.data[i*3+1];
-      b = (float)(unsigned int)(unsigned char) imgOrig.data[i*3+2];
+	cv::cvtColor(layers[0],colorDepth,CV_GRAY2RGB);
 
-      const HSV* hsvData =  RGB2HSV_getHSV (r,g,b);
+	Glib::RefPtr<Gdk::Pixbuf> imgBuffDepth =
+			Gdk::Pixbuf::create_from_data((const guint8*) colorDepth.data,
+					Gdk::COLORSPACE_RGB,
+					false,
+					8,
+					imageDepth.width,
+					imageDepth.height,
+					imageDepth.step);
 
-      if( hmax >= hsvData->H*DEGTORAD && hmin <= hsvData->H*DEGTORAD 
-	  && smax >= hsvData->S && smin <= hsvData->S
-	  && vmax >= hsvData->V && vmin <=  hsvData->V )
-      {
-	threshy->imageData[i] = 1;
-      }
-      else
-      {
-	imgHSV.data[i*3] = imgHSV.data[i*3+1] = imgHSV.data[i*3+2] = 0;
-	threshy->imageData[i] = 0;
-      }
-      
+	gtkimage_depth->clear();
+	gtkimage_depth->set(imgBuffDepth);
 
-    }
+	if (hsvFilter != NULL)
+	{
+		createImageHSV(imageDepth);
 
-    //Structure to hold blobs
-    CvBlobs blobs;
+		// Show HSV image
+		Glib::RefPtr<Gdk::Pixbuf> imgBuffHSV =
+				Gdk::Pixbuf::create_from_data((const guint8*)imgHSV.data,
+						Gdk::COLORSPACE_RGB,
+						false,
+						8,
+						imgHSV.size().width,
+						imgHSV.size().height,
+						imgHSV.step);
 
-    IplImage *iplOrig = new IplImage(imgOrig);
+		gtkimage_hsv->clear();
+		gtkimage_hsv->set(imgBuffHSV);
 
-    if (mFrameBlob)
-      cvReleaseImage(&mFrameBlob);
-    mFrameBlob=cvCreateImage(imgOrig.size(),8,3);
+		// Show Blob Image
 
-    IplImage *labelImg=cvCreateImage(imgOrig.size(),IPL_DEPTH_LABEL,1);
+		Glib::RefPtr<Gdk::Pixbuf> imgBuffBLOB =
+				Gdk::Pixbuf::create_from_data(
+						(guint8*)mFrameBlob->imageData,
+						Gdk::COLORSPACE_RGB,
+						false,
+						mFrameBlob->depth,
+						mFrameBlob->width,
+						mFrameBlob->height,
+						mFrameBlob->widthStep);
 
-    cvResize(iplOrig,mFrameBlob,CV_INTER_LINEAR );
+		gtkimage_blob->clear();
+		gtkimage_blob->set(imgBuffBLOB);
 
-    //Threshy is a binary image
-    cvSmooth(threshy,threshy,CV_MEDIAN,7,7);
+		//cvReleaseImage(&mFrameBlob);
 
-    //Finding the blobs
-    unsigned int result=cvLabel(threshy,labelImg,blobs);
+	}
 
-    //Rendering the blobs
-    cvRenderBlobs(labelImg,blobs,mFrameBlob,mFrameBlob);
+	displayFrameRate();
+	mainwindow->resize(1,1);
+	while (gtkmain.events_pending())
+		gtkmain.iteration();
+}
 
-    //Filter Blobs
-    cvFilterByArea(blobs,500,5000);    
+void Viewer::createImageHSV(const colorspaces::Image& imageDepth)
+{
+	float r,g,b;
 
-    double area = 0.0;
-    int x=0;
-    int y=0;
-    
-    for (CvBlobs::const_iterator it=blobs.begin(); it!=blobs.end(); ++it)
-    {
-      //std::cout << "BLOB found: " << it->second->area  <<std::endl;
+	imgHSV.create(imgOrig.size(), CV_8UC1);
+	imgOrig.copyTo(imgHSV);
 
-      double moment10 = it->second->m10;
-      double moment01 = it->second->m01;
+	IplImage *threshy=cvCreateImage(imgOrig.size(),8,1);
 
-      if (it->second->area >= area)
-      {      
-	area = it->second->area;
-	x = moment10/area;
-	y = moment01/area;
-      }
-      
-    }
+	for (int i=0;i< imgHSV.size().width*imgHSV.size().height; i++)
+	{
+		r = (float)(unsigned int)(unsigned char) imgOrig.data[i*3];
+		g = (float)(unsigned int)(unsigned char) imgOrig.data[i*3+1];
+		b = (float)(unsigned int)(unsigned char) imgOrig.data[i*3+2];
 
-    std::cout << "Max BLOB: " << area << ": " << x << " , " << y  <<std::endl;
+		const HSV* hsvData =  RGB2HSV_getHSV (r,g,b);
 
-    //cvShowImage("Live",mFrameBlob);
+		if( hmax >= hsvData->H*DEGTORAD && hmin <= hsvData->H*DEGTORAD
+				&& smax >= hsvData->S && smin <= hsvData->S
+				&& vmax >= hsvData->V && vmin <=  hsvData->V )
+		{
+			threshy->imageData[i] = 1;
+		}
+		else
+		{
+			imgHSV.data[i*3] = imgHSV.data[i*3+1] = imgHSV.data[i*3+2] = 0;
+			threshy->imageData[i] = 0;
+		}
 
-    if (area != 0)
-    {
-      Eigen::Vector3d pixel;
-      pixel(0) = x;
-      pixel(1) = y;
-      pixel(2) = 1.0;
-      
-      Eigen::Vector4d target;
-      
-      mCalibration->BackProjectWithDepth(pixel, imageDepth, target);
 
-    }
+	}
 
-    // Release and free memory
-    delete(iplOrig);
-    cvReleaseImage(&threshy);
-    cvReleaseImage(&labelImg);
+	//Structure to hold blobs
+	CvBlobs blobs;
 
-  }
+	IplImage *iplOrig = new IplImage(imgOrig);
 
-  void
-  Viewer::displayFrameRate()
-  {
-    double diff;
-    IceUtil::Time diffT;
+	if (mFrameBlob)
+		cvReleaseImage(&mFrameBlob);
+	mFrameBlob=cvCreateImage(imgOrig.size(),8,3);
 
-    currentFrameTime = IceUtil::Time::now();
-    diff = (currentFrameTime - oldFrameTime).toMilliSecondsDouble();
-    if (diff < 1000.0)
-      frameCount++;
-    else{
-      oldFrameTime = currentFrameTime;
-      fps = frameCount*1000.0/diff;
-      frameCount=0;
-      // Display the frame rate
-      std::stringstream fpsString;
-      fpsString << "fps = " << int(fps);
-      fpsLabel->set_label(fpsString.str());
-    }
-  }
+	IplImage *labelImg=cvCreateImage(imgOrig.size(),IPL_DEPTH_LABEL,1);
 
-  bool Viewer::on_eventbox_extrinsics_clicked(GdkEventButton * event)
-  {
+	cvResize(iplOrig,mFrameBlob,CV_INTER_LINEAR );
 
-    if (mCalibration){
-   
-      pthread_mutex_lock(&mutex);
-      bool res = mCalibration->addPatternPixel (Eigen::Vector3d ((int) event->x, 
-								 (int) event->y, 
-								 1.0),
-						mImageDepth);
+	//Threshy is a binary image
+	cvSmooth(threshy,threshy,CV_MEDIAN,7,7);
 
-      if (res == false)
-      {
-	Eigen::Vector3d p2D ((int) event->x, (int) event->y, 1.0);
-	Eigen::Vector4d p3D;
-	mCalibration->BackProjectWithDepth (p2D, mImageDepth, p3D);
-	mCalibration->test(p3D);
-      }
+	//Finding the blobs
+	unsigned int result=cvLabel(threshy,labelImg,blobs);
 
-      pthread_mutex_unlock(&mutex);
-    }
-  }
+	//Rendering the blobs
+	cvRenderBlobs(labelImg,blobs,mFrameBlob,mFrameBlob);
 
-  bool Viewer::on_eventbox_clicked(GdkEventButton * event)
-  {
-    int posX, posY;
-    float r,g,b;
-    posX = (int) event->x;
-    posY = (int) event->y;
-    
-    pthread_mutex_lock(&mutex);
-    
-    int index = posY*imgOrig.step+posX*imgOrig.channels();
-    r = (float)(unsigned int) (unsigned char)imgOrig.data[index];
-    g = (float)(unsigned int) (unsigned char)imgOrig.data[index+1];
-    b = (float)(unsigned int) (unsigned char)imgOrig.data[index+2]; 
-    
-    pthread_mutex_unlock(&mutex);
-    
-    
-    if (DEBUG) std::cout << "[RGB] -> " << r << " " << g << " " << b << std::endl;
-    hsvFilter = RGB2HSV_getHSV (r,g,b);
-    if (DEBUG) std::cout << "[HSV] -> " << hsvFilter->H << " " << hsvFilter->S << " " << hsvFilter->V << std::endl;
-    
-    // Calculate HSV Min y Max
-    hmax = hsvFilter->H*DEGTORAD + 0.2;
-    hmin = hsvFilter->H*DEGTORAD - 0.2;
-    if(hmax>6.28) hmax = 6.28;
-    if(hmin<0.0)  hmin = 0.0;
-    
-    smax = hsvFilter->S + 0.1;
-    smin = hsvFilter->S - 0.1;
-    if(smax > 1.0)
-      smax = 1.0;
-    if(smin < 0.0)
-      smin = 0.0;
-    
-    vmax = hsvFilter->V + 50.0;
-    vmin = hsvFilter->V - 50.0;
-    if(vmax > 255.0)
-      vmax = 255.0;
-    if(vmin < 0.0)
-      vmin = 0.0; 
-    
-    if (DEBUG)
-      std::cout << "H[min,max] - S[min,max] - V[min,max]: " <<
-       "[" << hmin << " " << hmax << "] " <<
-	"[" << smin << " " << smax << "] " <<
-	"[" << vmin << " " << vmax << "] " << std::endl;
-    
-    return true;
-  }
-  
-  void Viewer::on_bt_take_photo_clicked() 
-  {
-    intrinsicsEnable = 1;
+	//Filter Blobs
+	cvFilterByArea(blobs,500,5000);
 
-    // Get num of photos
-    std::string num (etNumPhoto->get_buffer()->get_text());
-    numPhoto = atoi(num.c_str());
-    std::cout << numPhoto << std::endl;
+	double area = 0.0;
+	int x=0;
+	int y=0;
 
-    // Get delay photo and Init time last photo
-    std::string sleep (etSleepPhoto->get_buffer()->get_text());
-    delayPhoto = atoi(sleep.c_str());
-    lastTimePhoto = IceUtil::Time::now();
+	for (CvBlobs::const_iterator it=blobs.begin(); it!=blobs.end(); ++it)
+	{
+		//std::cout << "BLOB found: " << it->second->area  <<std::endl;
 
-    tvStatus->get_buffer()->set_text("Init process, show the pattern!");
-  }
+		double moment10 = it->second->m10;
+		double moment01 = it->second->m01;
 
-  void Viewer::saveImage(const colorspaces::Image& imageColor)
-  {
-    
+		if (it->second->area >= area)
+		{
+			area = it->second->area;
+			x = moment10/area;
+			y = moment01/area;
+		}
 
-    IceUtil::Time currentTime = IceUtil::Time::now();    
-    if ((currentTime - lastTimePhoto).toSeconds() == delayPhoto)
-    {
+	}
 
-      // Check the directory
-      if ( !boost::filesystem::exists(pathImage)) 
-      {
-	boost::filesystem::path dir(pathImage);
-	if (!boost::filesystem::create_directory(dir))
-	  std::cout << "Error to create directory" << std::endl;
-      }
+	std::cout << "Max BLOB: " << area << ": " << x << " , " << y  <<std::endl;
 
+	//cvShowImage("Live",mFrameBlob);
 
-      lastTimePhoto = IceUtil::Time::now();
+	if (area != 0)
+	{
+		Eigen::Vector3d pixel;
+		pixel(0) = x;
+		pixel(1) = y;
+		pixel(2) = 1.0;
 
-      // Convert to gray
-      Mat gray(imageColor.size(), CV_8UC1);
-      cvtColor(imageColor, gray, CV_RGB2GRAY);
+		Eigen::Vector4d target;
 
-      // Save Image
-      std::stringstream filename;
-      filename << pathImage << "img" << contPhoto << ".jpg";      
-      imwrite( filename.str().c_str(), gray );
+		mCalibration->BackProjectWithDepth(pixel, imageDepth, target);
 
-      std::string msg = "Image saved: " + filename.str();
-      tvStatus->get_buffer()->set_text(msg.c_str());
-      
-      beep();
+	}
 
-      if (contPhoto < numPhoto)
-	contPhoto ++;
-      else
+	// Release and free memory
+	delete(iplOrig);
+	cvReleaseImage(&threshy);
+	cvReleaseImage(&labelImg);
+
+}
+
+void
+Viewer::displayFrameRate()
+{
+	double diff;
+	IceUtil::Time diffT;
+
+	currentFrameTime = IceUtil::Time::now();
+	diff = (currentFrameTime - oldFrameTime).toMilliSecondsDouble();
+	if (diff < 1000.0)
+		frameCount++;
+	else{
+		oldFrameTime = currentFrameTime;
+		fps = frameCount*1000.0/diff;
+		frameCount=0;
+		// Display the frame rate
+		std::stringstream fpsString;
+		fpsString << "fps = " << int(fps);
+		fpsLabel->set_label(fpsString.str());
+	}
+}
+
+bool Viewer::on_eventbox_extrinsics_clicked(GdkEventButton * event)
+{
+
+	if (mCalibration){
+
+		pthread_mutex_lock(&mutex);
+		bool res = mCalibration->addPatternPixel (Eigen::Vector3d ((int) event->x,
+				(int) event->y,
+				1.0),
+				mImageDepth);
+
+		if (res == false)
+		{
+			Eigen::Vector3d p2D ((int) event->x, (int) event->y, 1.0);
+			Eigen::Vector4d p3D;
+			mCalibration->BackProjectWithDepth (p2D, mImageDepth, p3D);
+			mCalibration->test(p3D);
+		}
+
+		pthread_mutex_unlock(&mutex);
+	}
+}
+
+bool Viewer::on_eventbox_clicked(GdkEventButton * event)
+{
+	int posX, posY;
+	float r,g,b;
+	posX = (int) event->x;
+	posY = (int) event->y;
+
+	pthread_mutex_lock(&mutex);
+
+	int index = posY*imgOrig.step+posX*imgOrig.channels();
+	r = (float)(unsigned int) (unsigned char)imgOrig.data[index];
+	g = (float)(unsigned int) (unsigned char)imgOrig.data[index+1];
+	b = (float)(unsigned int) (unsigned char)imgOrig.data[index+2];
+
+	pthread_mutex_unlock(&mutex);
+
+
+	if (DEBUG) std::cout << "[RGB] -> " << r << " " << g << " " << b << std::endl;
+	hsvFilter = RGB2HSV_getHSV (r,g,b);
+	if (DEBUG) std::cout << "[HSV] -> " << hsvFilter->H << " " << hsvFilter->S << " " << hsvFilter->V << std::endl;
+
+	// Calculate HSV Min y Max
+	hmax = hsvFilter->H*DEGTORAD + 0.2;
+	hmin = hsvFilter->H*DEGTORAD - 0.2;
+	if(hmax>6.28) hmax = 6.28;
+	if(hmin<0.0)  hmin = 0.0;
+
+	smax = hsvFilter->S + 0.1;
+	smin = hsvFilter->S - 0.1;
+	if(smax > 1.0)
+		smax = 1.0;
+	if(smin < 0.0)
+		smin = 0.0;
+
+	vmax = hsvFilter->V + 50.0;
+	vmin = hsvFilter->V - 50.0;
+	if(vmax > 255.0)
+		vmax = 255.0;
+	if(vmin < 0.0)
+		vmin = 0.0;
+
+	if (DEBUG)
+		std::cout << "H[min,max] - S[min,max] - V[min,max]: " <<
+		"[" << hmin << " " << hmax << "] " <<
+		"[" << smin << " " << smax << "] " <<
+		"[" << vmin << " " << vmax << "] " << std::endl;
+
+	return true;
+}
+
+void Viewer::on_bt_take_photo_clicked()
+{
+	intrinsicsEnable = 1;
+
+	// Get num of photos
+	std::string num (etNumPhoto->get_buffer()->get_text());
+	numPhoto = atoi(num.c_str());
+	std::cout << numPhoto << std::endl;
+
+	// Get delay photo and Init time last photo
+	std::string sleep (etSleepPhoto->get_buffer()->get_text());
+	delayPhoto = atoi(sleep.c_str());
+	lastTimePhoto = IceUtil::Time::now();
+
+	tvStatus->get_buffer()->set_text("Init process, show the pattern!");
+}
+
+void Viewer::saveImage(const colorspaces::Image& imageColor)
+{
+
+
+	IceUtil::Time currentTime = IceUtil::Time::now();
+	if ((currentTime - lastTimePhoto).toSeconds() == delayPhoto)
 	{
-	  intrinsicsEnable = 0;
-	  contPhoto = 1;
-	}	
-    }
 
-  }
+		// Check the directory
+		if ( !boost::filesystem::exists(pathImage))
+		{
+			boost::filesystem::path dir(pathImage);
+			if (!boost::filesystem::create_directory(dir))
+				std::cout << "Error to create directory" << std::endl;
+		}
 
-  void Viewer::beep()
-  {
-    system("mplayer beep.wav &> /dev/null");
-  }
-  
-  void Viewer::on_bt_intrinsic() 
-  {
 
+		lastTimePhoto = IceUtil::Time::now();
 
-    tvStatus->get_buffer()->set_text("Getting intrinsics ...");
+		// Convert to gray
+		Mat gray(imageColor.size(), CV_8UC1);
+		cvtColor(imageColor, gray, CV_RGB2GRAY);
 
-    Size boardSize, imageSize; 
-    Mat cameraMatrix, distCoeffs;
-    vector<vector<Point2f> > imagePoints;
+		// Save Image
+		std::stringstream filename;
+		filename << pathImage << "img" << contPhoto << ".jpg";
+		imwrite( filename.str().c_str(), gray );
 
+		std::string msg = "Image saved: " + filename.str();
+		tvStatus->get_buffer()->set_text(msg.c_str());
 
-    // TODO: get this data from user by GUI
-    boardSize.width = 9;
-    boardSize.height= 6;
+		beep();
 
-    //flag
-    int flag = 0;
-    flag |= CV_CALIB_FIX_PRINCIPAL_POINT;
-    flag |= CV_CALIB_ZERO_TANGENT_DIST;
-    flag |= CV_CALIB_FIX_ASPECT_RATIO;
+		if (contPhoto < numPhoto)
+			contPhoto ++;
+		else
+		{
+			intrinsicsEnable = 0;
+			contPhoto = 1;
+		}
+	}
 
-    // List of images
-    boost::filesystem::directory_iterator end_itr; 
-    for ( boost::filesystem::directory_iterator itr( pathImage ); itr != end_itr; ++itr )
-    {
+}
 
-      Mat view;
-      view = imread(itr->path().c_str(), CV_LOAD_IMAGE_COLOR);
+void Viewer::beep()
+{
+	system("mplayer beep.wav &> /dev/null");
+}
 
-      imageSize = view.size();
+void Viewer::on_bt_intrinsic()
+{
 
-      vector<Point2f> pointBuf;
-      bool found = findChessboardCorners( view, boardSize, pointBuf, 
-					  CV_CALIB_CB_ADAPTIVE_THRESH | 
-					  CV_CALIB_CB_FAST_CHECK | 
-					  CV_CALIB_CB_NORMALIZE_IMAGE);
 
-      if (found)
-      {
-	
-	Mat viewGray;
-	cvtColor(view, viewGray, CV_BGR2GRAY);
-	cornerSubPix( viewGray, pointBuf, Size(11,11),
-		      Size(-1,-1), TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
+	tvStatus->get_buffer()->set_text("Getting intrinsics ...");
 
-	imagePoints.push_back(pointBuf);
+	Size boardSize, imageSize;
+	Mat cameraMatrix, distCoeffs;
+	vector<vector<Point2f> > imagePoints;
 
-	// Draw the corners.
-	drawChessboardCorners( view, boardSize, Mat(pointBuf), found );
-	//imshow("Image View", view);
-	
-      }
-    }
 
+	// TODO: get this data from user by GUI
+	boardSize.width = 9;
+	boardSize.height= 6;
 
-    mCalibration->runCalibrationAndSave(boardSize, 20.0, flag, imageSize,  
-					cameraMatrix, distCoeffs, imagePoints);
+	//flag
+	int flag = 0;
+	flag |= CV_CALIB_FIX_PRINCIPAL_POINT;
+	flag |= CV_CALIB_ZERO_TANGENT_DIST;
+	flag |= CV_CALIB_FIX_ASPECT_RATIO;
 
-    std::cout << std::endl << cameraMatrix << std::endl;
+	// List of images
+	boost::filesystem::directory_iterator end_itr;
+	for ( boost::filesystem::directory_iterator itr( pathImage ); itr != end_itr; ++itr )
+	{
 
-    std::stringstream matrixStr;
+		Mat view;
+		view = imread(itr->path().c_str(), CV_LOAD_IMAGE_COLOR);
 
-    matrixStr << "Intrinsic Matrix" << std::endl;
-    matrixStr << "================" << std::endl << std::endl;
+		imageSize = view.size();
 
-    for (int i=0; i<cameraMatrix.cols ; i++)
-    {
-      for (int j=0; j<cameraMatrix.rows; j++)
-      {
-	matrixStr << cameraMatrix.at<double>(i,j) << "\t\t";
-	if (i==2 && j==0)
-	  matrixStr << "\t";
-      }
-      matrixStr<<std::endl;
+		vector<Point2f> pointBuf;
+		bool found = findChessboardCorners( view, boardSize, pointBuf,
+				CV_CALIB_CB_ADAPTIVE_THRESH |
+				CV_CALIB_CB_FAST_CHECK |
+				CV_CALIB_CB_NORMALIZE_IMAGE);
 
-      tvStatus->get_buffer()->set_text(matrixStr.str().c_str());
-    }
+		if (found)
+		{
 
-    
-  }
+			Mat viewGray;
+			cvtColor(view, viewGray, CV_BGR2GRAY);
+			cornerSubPix( viewGray, pointBuf, Size(11,11),
+					Size(-1,-1), TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
 
+			imagePoints.push_back(pointBuf);
 
+			// Draw the corners.
+			drawChessboardCorners( view, boardSize, Mat(pointBuf), found );
+			//imshow("Image View", view);
+
+		}
+	}
+
+
+	mCalibration->runCalibrationAndSave(boardSize, 20.0, flag, imageSize,
+			cameraMatrix, distCoeffs, imagePoints);
+
+	std::cout << std::endl << cameraMatrix << std::endl;
+
+	std::stringstream matrixStr;
+
+	matrixStr << "Intrinsic Matrix" << std::endl;
+	matrixStr << "================" << std::endl << std::endl;
+
+	for (int i=0; i<cameraMatrix.cols ; i++)
+	{
+		for (int j=0; j<cameraMatrix.rows; j++)
+		{
+			matrixStr << cameraMatrix.at<double>(i,j) << "\t\t";
+			if (i==2 && j==0)
+				matrixStr << "\t";
+		}
+		matrixStr<<std::endl;
+
+		tvStatus->get_buffer()->set_text(matrixStr.str().c_str());
+	}
+
+
+}
+
+
 }//namespace

Modified: trunk/src/stable/components/rgbdCalibrator/viewer.h
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/viewer.h	2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/rgbdCalibrator/viewer.h	2013-10-07 13:37:13 UTC (rev 1012)
@@ -28,10 +28,10 @@
 #include <IceUtil/Thread.h>
 #include <IceUtil/Time.h>
 #include <string>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
 #include <cmath>
 #include <jderobot/camera.h>
-#include <colorspaces/colorspaces.h>
+#include <visionlib/colorspaces/colorspaces.h>
 #include "calibration.h"
 
 using namespace cv;

Modified: trunk/src/stable/components/rgbdManualCalibrator/controller.h
===================================================================
--- trunk/src/stable/components/rgbdManualCalibrator/controller.h	2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/rgbdManualCalibrator/controller.h	2013-10-07 13:37:13 UTC (rev 1012)
@@ -29,7 +29,7 @@
 #include <iostream>
 #include <opencv/cv.h>
 #include <progeo/progeo.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
 #include <Ice/Ice.h>
 #include <IceUtil/IceUtil.h>
 #include <opencv/cv.h>

Modified: trunk/src/stable/components/rgbdManualCalibrator/rgbdManualCalibrator.cpp
===================================================================
--- trunk/src/stable/components/rgbdManualCalibrator/rgbdManualCalibrator.cpp	2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/rgbdManualCalibrator/rgbdManualCalibrator.cpp	2013-10-07 13:37:13 UTC (rev 1012)
@@ -25,7 +25,7 @@
 #include <Ice/Ice.h>
 #include <IceUtil/IceUtil.h>
 #include <jderobot/camera.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
 #include "view.h"
 #include "controller.h"
 #include "common.h"

Modified: trunk/src/stable/components/rgbdManualCalibrator/view.h
===================================================================
--- trunk/src/stable/components/rgbdManualCalibrator/view.h	2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/rgbdManualCalibrator/view.h	2013-10-07 13:37:13 UTC (rev 1012)
@@ -30,7 +30,7 @@
 #include <IceUtil/Thread.h>
 #include <IceUtil/Time.h>
 #include "controller.h"
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
 #include "drawarea.h"
 #include "common.h"
 //eclipse

Modified: trunk/src/stable/libs/geometry/math/Segment3D.cpp
===================================================================
--- trunk/src/stable/libs/geometry/math/Segment3D.cpp	2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/libs/geometry/math/Segment3D.cpp	2013-10-07 13:37:13 UTC (rev 1012)
@@ -84,7 +84,7 @@
 
   delete(vector);
 
-  Point3D* res = new Point3D(x,y,z);
+  Point3D* res = new Point3D(x,y,z,1.0);
   return res;
 }
 

Modified: trunk/src/stable/libs/geometry/progeo/Progeo.cpp
===================================================================
--- trunk/src/stable/libs/geometry/progeo/Progeo.cpp	2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/libs/geometry/progeo/Progeo.cpp	2013-10-07 13:37:13 UTC (rev 1012)
@@ -24,6 +24,7 @@
 #include "Progeo.h"
 #include <math.h>
 
+
 namespace Progeo {
 
 Progeo::Progeo()
@@ -46,6 +47,7 @@
 	foa(3) = 1.;
 
 	roll = 0.;
+	skew = 0.;
 
 }
 
@@ -357,7 +359,7 @@
 
 void Progeo::displayCameraInfo() {
 
-	printf("------------------------------------------------------\n");
+	printf("\n----------------------- PROGEO C++ ---------------------\n");
 	printf("Camera %s\n\n", this->name.c_str());
 	printf("     Position: (X,Y,Z,H)=(%.1f,%.1f,%.1f,%.1f)\n",   position(0),
 			position(1),
@@ -408,14 +410,14 @@
 void Progeo::backproject(Eigen::Vector3d point, Eigen::Vector4d& pro)
 {
 	//GRAPHIC_TO_OPTICAL
-	int opX = this->rows -1 -point(1);
-	int opY = point(0);
+	//int opX = this->rows -1 -point(1);
+	//int opY = point(0);
 
 	Eigen::Matrix3d ik;
 	ik = K;
 	ik = ik.inverse().eval();
 
-	Eigen::Vector3d Pi(opX*K(0,0)/point(2), opY*K(0,0)/point(2),K(0,0));
+	Eigen::Vector3d Pi(pro(0)*K(0,0)/point(2), pro(1)*K(0,0)/point(2),K(0,0));
 
 	Eigen::Vector3d a;
 	a = ik*Pi;
@@ -634,9 +636,6 @@
 
 	xmlDocSetRootElement(doc, root_node);
 
-	//updateKMatrix();
-	//updateRTMatrix();
-
 	// Camera position
 	xmlNewChild(node_position, NULL, BAD_CAST "x", BAD_CAST double2char(position(0)));
 	xmlNewChild(node_position, NULL, BAD_CAST "y", BAD_CAST double2char(position(1)));

Modified: trunk/src/stable/libs/parallelIce/cameraClient.h
===================================================================
--- trunk/src/stable/libs/parallelIce/cameraClient.h	2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/libs/parallelIce/cameraClient.h	2013-10-07 13:37:13 UTC (rev 1012)
@@ -25,7 +25,7 @@
 #include <IceUtil/IceUtil.h>
 #include <iostream>
 #include <Ice/Ice.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
 #include <jderobot/camera.h>
 #include <cv.h>
 #include <sstream>

Modified: trunk/src/stable/libs/visionlib/CMakeLists.txt
===================================================================
--- trunk/src/stable/libs/visionlib/CMakeLists.txt	2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/libs/visionlib/CMakeLists.txt	2013-10-07 13:37:13 UTC (rev 1012)
@@ -1,3 +1,6 @@
+
+add_subdirectory (colorspaces)
+
 SET( SOURCE_FILES cvfast.cpp geometry.cpp image.cpp linesDetection.cpp)
 include_directories( ${LIBS_DIR}/)
 add_library (visionlib  SHARED ${SOURCE_FILES})

Modified: trunk/src/stable/libs/visionlib/colorspaces/CMakeLists.txt
===================================================================
--- trunk/src/stable/libs/colorspaces/CMakeLists.txt	2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/libs/visionlib/colorspaces/CMakeLists.txt	2013-10-07 13:37:13 UTC (rev 1012)
@@ -1,6 +1,4 @@
 
-
-
 add_library (colorspacesmm STATIC imagecv.cpp imagecv.h uncopyable.h)
 
 TARGET_LINK_LIBRARIES(colorspacesmm ${OpenCV_LIBRARIES})

Modified: trunk/src/stable/libs/visionlib/linesDetection.h
===================================================================
--- trunk/src/stable/libs/visionlib/linesDetection.h	2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/libs/visionlib/linesDetection.h	2013-10-07 13:37:13 UTC (rev 1012)
@@ -27,7 +27,7 @@
 #include <opencv/cv.h>
 #include <opencv/highgui.h>
 #include <stdio.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
 #include "geometry.h"
 #include "cvfast.h"
 #include "structs.h"



More information about the Jderobot-admin mailing list