[Jderobot-admin] jderobot-r970 - in trunk/src/stable/components: . rgbdCalibrator

rocapal en jderobot.org rocapal en jderobot.org
Lun Ago 12 12:54:01 CEST 2013


Author: rocapal
Date: 2013-08-12 12:53:01 +0200 (Mon, 12 Aug 2013)
New Revision: 970

Added:
   trunk/src/stable/components/rgbdCalibrator/
   trunk/src/stable/components/rgbdCalibrator/CMakeLists.txt
   trunk/src/stable/components/rgbdCalibrator/beep.wav
   trunk/src/stable/components/rgbdCalibrator/calibration.cpp
   trunk/src/stable/components/rgbdCalibrator/calibration.h
   trunk/src/stable/components/rgbdCalibrator/geoUtils.cpp
   trunk/src/stable/components/rgbdCalibrator/geoUtils.h
   trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.cfg
   trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.cpp
   trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.glade
   trunk/src/stable/components/rgbdCalibrator/viewer.cpp
   trunk/src/stable/components/rgbdCalibrator/viewer.h
Log:
#27 intrinsics params obtained with opencv


Added: trunk/src/stable/components/rgbdCalibrator/CMakeLists.txt
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/CMakeLists.txt	                        (rev 0)
+++ trunk/src/stable/components/rgbdCalibrator/CMakeLists.txt	2013-08-12 10:53:01 UTC (rev 970)
@@ -0,0 +1,25 @@
+SET( SOURCE_FILES rgbdCalibrator.cpp viewer.cpp calibration.cpp geoUtils.cpp)
+
+add_definitions(-DGLADE_DIR="${gladedir}")
+
+include_directories(
+    ${INTERFACES_CPP_DIR}
+    ${LIBS_DIR}
+    ${CMAKE_CURRENT_SOURCE_DIR}
+)
+
+add_executable (rgbdCalibrator ${SOURCE_FILES})
+
+TARGET_LINK_LIBRARIES(rgbdCalibrator
+    ${CMAKE_THREAD_LIBS_INIT}
+    ${alut_LIBRARIES}
+    ${OpenCV_LIBRARIES}
+    ${gtkmm_LIBRARIES}
+    ${libglademm_LIBRARIES}
+    ${LIBS_DIR}/colorspaces/libcolorspacesmm.so
+    ${INTERFACES_CPP_DIR}/jderobot/libJderobotInterfaces.so
+    ${ZeroCIce_LIBRARIES}
+    ${Boost_LIBRARIES}
+    ${LIBS_DIR}/progeo/libprogeo.so
+    ${LIBS_DIR}/geometry/libgeometry.so
+)

Added: trunk/src/stable/components/rgbdCalibrator/beep.wav
===================================================================
(Binary files differ)


Property changes on: trunk/src/stable/components/rgbdCalibrator/beep.wav
___________________________________________________________________
Added: svn:mime-type
   + application/octet-stream

Added: trunk/src/stable/components/rgbdCalibrator/calibration.cpp
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/calibration.cpp	                        (rev 0)
+++ trunk/src/stable/components/rgbdCalibrator/calibration.cpp	2013-08-12 10:53:01 UTC (rev 970)
@@ -0,0 +1,464 @@
+/*
+ *
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see http://www.gnu.org/licenses/. 
+ *
+ *  Authors : Roberto Calvo <rocapal [at] gsyc [dot] urjc [dot] es>
+ *
+ */
+
+#include <calibration.h>
+#include <iostream>
+#include "geoUtils.h"
+#include <jderobot/progeo/Progeo.h>
+#include <jderobot/math/Point3D.h>
+#include <jderobot/math/Segment3D.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/
+
+
+  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);
+
+    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)
+  {
+
+    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;
+      }
+  }
+
+
+
+  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);
+  }
+
+
+  // Extrinsics
+  void Calibration::extrinsics (const Mat& kMatrix, 
+				const jderobot::ImageDataPtr depthData)
+
+  {
+
+    /*
+    
+    TPinHoleCamera m_progeoCam;
+
+    // Camera Position
+    HPoint3D positionCam;
+
+    positionCam.X= 0;
+    positionCam.Y= 0;
+    positionCam.Z= 0;
+    positionCam.H= 0.0;
+
+    m_progeoCam.position = positionCam;
+
+    // K Matrix
+    m_progeoCam.k11 = kMatrix.at<double>(0,0);
+    m_progeoCam.k12 = kMatrix.at<double>(0,1);
+    m_progeoCam.k13 = kMatrix.at<double>(0,2);
+    m_progeoCam.k14 = 0;
+    
+    m_progeoCam.k21 = kMatrix.at<double>(1,0);
+    m_progeoCam.k22 = kMatrix.at<double>(1,1);
+    m_progeoCam.k23 = kMatrix.at<double>(1,2);
+    m_progeoCam.k24 = 0;
+
+    m_progeoCam.k31 = kMatrix.at<double>(2,0);
+    m_progeoCam.k32 = kMatrix.at<double>(2,1);
+    m_progeoCam.k33 = kMatrix.at<double>(2,2);
+    m_progeoCam.k34 = 0;
+
+    // RT Matrix
+    m_progeoCam.rt11 = 1;
+    m_progeoCam.rt12 = 0;
+    m_progeoCam.rt13 = 0; 
+    m_progeoCam.rt14 = 0;
+    
+    m_progeoCam.rt21 = 0;
+    m_progeoCam.rt22 = 1;
+    m_progeoCam.rt23 = 0;
+    m_progeoCam.rt24 = 0;
+    
+    m_progeoCam.rt31 = 0;
+    m_progeoCam.rt32 = 0;
+    m_progeoCam.rt33 = 1;
+    m_progeoCam.rt34 = 0;
+    
+    m_progeoCam.rt41 = 0;
+    m_progeoCam.rt42 = 0;
+    m_progeoCam.rt43 = 0;
+    m_progeoCam.rt44 = 1;
+
+
+    display_camerainfo(m_progeoCam);
+    // backproject
+    HPoint3D point3D;
+    HPoint2D point2D;
+
+    point2D.x = 180,0;
+    point2D.y = 180.0;
+    point2D.h = 1.0;
+    
+    printf ("Pto en 2D: %2.2f %2.2f %2.2f \n",point2D.x,point2D.y,point2D.h);
+
+    if (backproject(&point3D,point2D,m_progeoCam)!=-1)
+      printf("Backproject a 3D (de nuevo): %.2f %.2f %.2f %.2f \n",point3D.X, point3D.Y, point3D.Z, point3D.H);
+
+
+    float depth = (int)depthData->pixelData[((depthData->description->width*(int)point2D.y)+(int)point2D.x)*3+1]<<8 | 
+      (int)depthData->pixelData[((depthData->description->width*(int)point2D.y)+(int)point2D.x)*3+2];
+
+
+    std::cout << depth << std::endl;
+    std::cout << depthData->description->width << std::endl;
+
+    HPoint2D p2;
+    if (project(point3D, &p2, m_progeoCam))
+      printf("Project a 2D (de nuevo): %2.2f,%2.2f,%2.2f \n",p2.x,p2.y,p2.h);
+
+    point3D.X = 150.64;
+    point3D.Y = 444.57;
+    point3D.Z = 1000.0;
+    point3D.H = 1.0;
+
+    if (project(point3D, &p2, m_progeoCam))
+      printf("Project a 2D (): %2.2f,%2.2f,%2.2f \n",p2.x,p2.y,p2.h);
+
+    std::cout << "_----------------------_" << std::endl;
+    point2D.x = 180,0;
+    point2D.y = 180.0;
+    point2D.h = 1.0;
+
+    printf ("Pixek 2D: %2.2f %2.2f %2.2f \n",point2D.x,point2D.y,point2D.h);
+
+    if (backproject(&point3D,point2D,m_progeoCam)!=-1)
+      printf("Backproject a 3D (de nuevo): %.2f %.2f %.2f %.2f \n",point3D.X, point3D.Y, point3D.Z, point3D.H);
+
+    HPoint3D orig;
+    orig.X = 0.;
+    orig.Y = 0.;
+    orig.Z = 0.;
+    orig.H = 1.;
+    
+    GeoUtils* gUtils = new GeoUtils();
+    HPoint3D vector = gUtils->getVector(orig, point3D);
+    
+    GeoUtils::Line3D* line3D = new GeoUtils::Line3D(orig, vector);
+
+    HPoint3D p3 = line3D->getPointByZ(2000.0);
+
+    printf("getPointByZ : %.2f %.2f %.2f %.2f \n",p3.X, p3.Y, p3.Z, p3.H);
+
+    if (project(p3, &p2, m_progeoCam))
+      printf("Project a 2D (): %2.2f,%2.2f,%2.2f \n",p2.x,p2.y,p2.h);
+    
+    delete(line3D);
+    delete (gUtils);
+    
+    
+    std::cout << "_----------------------_" << std::endl;
+
+    */
+
+    /*
+
+    math::Vector3H pos;
+    pos.vector(0) = 0.0;
+    pos.vector(1) = 0.0;
+    pos.vector(2) = 0.0;
+    pos.vector(3) = 0.0;
+
+    math::Matriz3x3 K;
+    K.getMatriz()(0,0) = kMatrix.at<double>(0,0);
+    K.getMatriz()(0,1) = kMatrix.at<double>(0,1);
+    K.getMatriz()(0,2) = kMatrix.at<double>(0,2);
+
+    K.getMatriz()(1,0) = kMatrix.at<double>(1,0);
+    K.getMatriz()(1,1) = kMatrix.at<double>(1,1);
+    K.getMatriz()(1,2) = kMatrix.at<double>(1,2);
+
+    K.getMatriz()(2,0) = kMatrix.at<double>(2,0);
+    K.getMatriz()(2,1) = kMatrix.at<double>(2,1);
+    K.getMatriz()(2,2) = kMatrix.at<double>(2,2);
+
+    math::Matriz4x4 RT;
+    RT.getMatrix()(0,0) = 1.;
+    RT.getMatrix()(0,1) = 0.;
+    RT.getMatrix()(0,2) = 0.;
+    RT.getMatrix()(0,3) = 0.;
+
+    RT.getMatrix()(1,0) = 0.;
+    RT.getMatrix()(1,1) = 1.;
+    RT.getMatrix()(1,2) = 0.;
+    RT.getMatrix()(1,3) = 0.;
+
+    RT.getMatrix()(2,0) = 0.;
+    RT.getMatrix()(2,1) = 0.;
+    RT.getMatrix()(2,2) = 1.;
+    RT.getMatrix()(2,3) = 0.;
+
+    RT.getMatrix()(3,0) = 0.;
+    RT.getMatrix()(3,1) = 0.;
+    RT.getMatrix()(3,2) = 0.;
+    RT.getMatrix()(3,3) = 1.;
+
+    
+    Progeo::Progeo* progeo = new Progeo::Progeo(pos, K, RT, 320, 240);
+    progeo->display_camerainfo();
+
+    math::Vector3H p3D;
+    math::Vector2H pixel (10.0, 100.0, 1.0); 
+    
+    progeo->backproject(pixel, p3D);
+
+    std::cout << p3D << std::endl;
+
+    HPoint3D orig;
+    orig.X = 0.;
+    orig.Y = 0.;
+    orig.Z = 0.;
+    orig.H = 1.;
+    
+    HPoint3D point3D;
+    point3D.X = p3D.getX();
+    point3D.Y = p3D.getY();
+    point3D.Z = p3D.getZ();
+    point3D.H = 1.;
+
+    GeoUtils* gUtils = new GeoUtils();
+    HPoint3D vector = gUtils->getVector(orig, point3D);
+    
+    GeoUtils::Line3D* line3D = new GeoUtils::Line3D(orig, vector);
+
+    HPoint3D p3 = line3D->getPointByZ(2000.0);
+
+    printf("getPointByZ : %.2f %.2f %.2f %.2f \n",p3.X, p3.Y, p3.Z, p3.H);
+
+
+    p3D.setX(p3.X);
+    p3D.setY(p3.Y);
+    p3D.setZ(p3.Z);
+    p3D.setH(1.0);
+
+    std::cout << p3D << std::endl;
+
+    math::Vector2H pixel2;
+    progeo->project(p3D, pixel2);
+
+    std::cout << pixel2.getX() << " " << pixel2.getY() << std::endl;
+
+    delete(line3D);
+    delete (gUtils);
+    delete(progeo);
+
+    */
+
+    Eigen::Vector4d posCamera;
+    posCamera(0) = double(0.0);
+    posCamera(1) = double(0.0);
+    posCamera(2) = double(0.0);
+
+
+    Eigen::Matrix3d K;
+    K(0,0) = kMatrix.at<double>(0,0);
+    K(0,1) = kMatrix.at<double>(0,1);
+    K(0,2) = kMatrix.at<double>(0,2);
+
+    K(1,0) = kMatrix.at<double>(1,0);
+    K(1,1) = kMatrix.at<double>(1,1);
+    K(1,2) = kMatrix.at<double>(1,2);
+
+    K(2,0) = kMatrix.at<double>(2,0);
+    K(2,1) = kMatrix.at<double>(2,1);
+    K(2,2) = kMatrix.at<double>(2,2);
+
+    Eigen::Matrix4d RT;
+    RT(0,0) = double(1.);
+    RT(0,1) = double(0.);
+    RT(0,2) = double(0.);
+    RT(0,3) = double(0.);
+
+    RT(1,0) = double(0.);
+    RT(1,1) = double(1.);
+    RT(1,2) = double(0.);
+    RT(1,3) = double(0.);
+
+    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.);
+
+
+    Progeo::Progeo* progeo = new Progeo::Progeo(posCamera, K, RT, 320, 240);
+    progeo->display_camerainfo();   
+
+    Eigen::Vector4d p3D;
+    Eigen::Vector3d pixel;
+
+    pixel(0) = double(180.);
+    pixel(1) = double(180.); 
+    pixel(2) = double(1.);
+
+    progeo->backproject(pixel, p3D);
+
+    std::cout << "Punto 3D: " << p3D << std::endl; 
+
+    progeo->project(p3D, pixel);   
+
+    std::cout << "Pixel 2D: " << pixel << std::endl;
+     
+    Point3D *pStart = new Point3D(0.0,0.0,0.0);
+    Point3D *pEnd = new Point3D(p3D);
+
+    Segment3D *segment = new Segment3D(*pStart,*pEnd);
+    Point3D *nP3D = segment->getPointByZ(200.0);
+
+    std::cout << "New Point 3D with Z=200.0 " << *nP3D << std::endl;
+    
+    progeo->project(nP3D->getPoint(), pixel);
+
+    std::cout << "Project Pixel\n" << pixel << std::endl;
+
+    delete(pStart);
+    delete(pEnd);
+    delete(nP3D);
+    delete(segment);
+    delete(progeo);
+  }
+
+}

Added: trunk/src/stable/components/rgbdCalibrator/calibration.h
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/calibration.h	                        (rev 0)
+++ trunk/src/stable/components/rgbdCalibrator/calibration.h	2013-08-12 10:53:01 UTC (rev 970)
@@ -0,0 +1,93 @@
+/*
+ *
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see http://www.gnu.org/licenses/. 
+ *
+ *  Authors : Roberto Calvo <rocapal [at] gsyc [dot] urjc [dot] es>
+ *
+ */
+
+#ifndef RGBDCALIBRATOR_CALIBRATION_H
+#define RGBDCALIBRATOR_CALIBRATION_H
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/calib3d/calib3d.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <cmath>
+#include <string>
+#include <colorspaces/colorspacesmm.h>
+#include <jderobot/camera.h>
+
+using namespace cv;
+
+namespace rgbdCalibrator
+{
+
+  class Calibration
+  {
+
+  public:
+
+    Calibration();
+    ~Calibration();
+
+    enum Pattern { NOT_EXISTING, CHESSBOARD, CIRCLES_GRID, ASYMMETRIC_CIRCLES_GRID };
+
+    bool runCalibrationAndSave(Size &boardSize, 
+			       float squareSize, 
+			       int flag, 
+			       Size imageSize, 
+			       Mat&  cameraMatrix, 
+			       Mat& distCoeffs,
+			       vector<vector<Point2f> > imagePoints );
+
+    void extrinsics (const Mat& kMatrix, const jderobot::ImageDataPtr depth);
+    
+  private:
+
+
+    void calcBoardCornerPositions(Size boardSize, 
+				  float squareSize, 
+				  vector<Point3f>& corners,  
+				  Pattern patternType /*= Settings::CHESSBOARD*/);
+
+    double 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);
+
+    bool 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);
+    
+  };
+
+
+}
+
+
+#endif //RGBDCALIBRATOR_CALIBRATION_H 

Added: trunk/src/stable/components/rgbdCalibrator/geoUtils.cpp
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/geoUtils.cpp	                        (rev 0)
+++ trunk/src/stable/components/rgbdCalibrator/geoUtils.cpp	2013-08-12 10:53:01 UTC (rev 970)
@@ -0,0 +1,75 @@
+/*
+ *
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see http://www.gnu.org/licenses/. 
+ *
+ *  Authors : Roberto Calvo <rocapal [at] gsyc [dot] urjc [dot] es>
+ *
+ */
+
+#include "geoUtils.h"
+
+namespace rgbdCalibrator
+{
+
+  GeoUtils::Line3D::Line3D ()
+  {
+  }
+
+  GeoUtils::Line3D::Line3D (HPoint3D p, HPoint3D v)
+  {
+    point = p;
+    vector = v;
+  }
+
+  bool GeoUtils::Line3D::isPointInLine (HPoint3D p)
+  {
+    return ((p.X - point.X)/vector.X) == 
+      ((p.Y - point.Y)/vector.Y) ==
+      ((p.Z - point.Z)/vector.Z);
+  }
+
+  HPoint3D GeoUtils::Line3D::getPointByZ (float z)
+  {
+    HPoint3D res;
+    t = (z - point.Z)/ vector.Z;
+
+    res.X = point.X + t*vector.X;
+    res.Y = point.Y + t*vector.Y;
+    res.Z = z;
+    res.H = 1.0;
+    
+    return res;
+  }
+
+
+  GeoUtils::GeoUtils ()
+  {
+  }
+
+  HPoint3D GeoUtils::getVector (HPoint3D p0, HPoint3D p1)
+  {
+    HPoint3D vector;
+    vector.X = p0.X - p1.X;
+    vector.Y = p0.Y - p1.Y;
+    vector.Z = p0.Z - p1.Z;
+    vector.H = 1.;
+
+    return vector;
+  }
+
+
+
+}

Added: trunk/src/stable/components/rgbdCalibrator/geoUtils.h
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/geoUtils.h	                        (rev 0)
+++ trunk/src/stable/components/rgbdCalibrator/geoUtils.h	2013-08-12 10:53:01 UTC (rev 970)
@@ -0,0 +1,76 @@
+/*
+ *
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see http://www.gnu.org/licenses/. 
+ *
+ *  Authors : Roberto Calvo <rocapal [at] gsyc [dot] urjc [dot] es>
+ *
+ */
+
+#include <iostream>
+#include "viewer.h"
+#include <progeo/progeo.h>
+
+namespace rgbdCalibrator
+{
+
+  class GeoUtils
+  {
+
+  public:
+
+    class Line3D
+    {
+    public:
+
+      Line3D();
+
+      Line3D(HPoint3D p, HPoint3D v);
+
+      bool isPointInLine (HPoint3D p);
+      
+      HPoint3D getPointByZ (float z);
+      
+
+    private:
+      // parametric equations
+      // (x,y,z) = (x0,y0,z0) + t(x1,y1,z1)
+      // 
+      // (x0,y0,z0) is a point
+      // (x1,y1,z1) is a director vector
+      
+      // catesian equations
+      // x = x0 + t*x1
+      // y = y0 + t*y1
+      // z = z0 + t*z1
+
+      HPoint3D point;
+      HPoint3D vector;
+      
+      float t;
+
+    };
+    
+
+    GeoUtils ();
+    HPoint3D getVector (HPoint3D p0, HPoint3D p1);
+
+  private:
+
+  };
+
+
+
+}

Added: trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.cfg
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.cfg	                        (rev 0)
+++ trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.cfg	2013-08-12 10:53:01 UTC (rev 970)
@@ -0,0 +1,2 @@
+rgbdCalibrator.CameraColor.Proxy=cameraA:default -h localhost -p 9998
+rgbdCalibrator.CameraDepth.Proxy=cameraB:default -h localhost -p 9998

Added: trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.cpp
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.cpp	                        (rev 0)
+++ trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.cpp	2013-08-12 10:53:01 UTC (rev 970)
@@ -0,0 +1,106 @@
+/*
+ *
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see http://www.gnu.org/licenses/. 
+ *
+ *  Authors : Roberto Calvo <rocapal [at] gsyc [dot] urjc [dot] es>
+ *
+ */
+
+#include <iostream>
+#include <Ice/Ice.h>
+#include <IceUtil/IceUtil.h>
+#include <jderobot/camera.h>
+#include <colorspaces/colorspacesmm.h>
+#include "viewer.h"
+#include <cv.h>
+#include <highgui.h>
+
+
+
+int main(int argc, char** argv){
+  int status;
+  rgbdCalibrator::Viewer viewer;
+  Ice::CommunicatorPtr ic;
+
+  try{
+    ic = Ice::initialize(argc,argv);
+    Ice::ObjectPrx base = ic->propertyToProxy("rgbdCalibrator.CameraColor.Proxy");
+    if (0==base)
+      throw "Could not create proxy";
+
+    /*cast to CameraPrx*/
+    jderobot::CameraPrx cprxColor = jderobot::CameraPrx::checkedCast(base);
+    if (0==cprxColor)
+      throw "Invalid proxy";
+
+    Ice::ObjectPrx base2 = ic->propertyToProxy("rgbdCalibrator.CameraDepth.Proxy");
+    if (base2 == 0)
+      throw "Could not create proxy";
+
+    /*cast to CameraPrx*/
+    jderobot::CameraPrx cprxDepth = jderobot::CameraPrx::checkedCast(base2);
+    if (0==cprxDepth)
+      throw "Invalid proxy";
+
+    
+    while(viewer.isVisible()){
+
+      
+      jderobot::ImageDataPtr data = cprxColor->getImageData();
+      colorspaces::Image::FormatPtr fmt = colorspaces::Image::Format::searchFormat(data->description->format);
+      if (!fmt)
+	throw "Format not supported";
+
+      colorspaces::Image imgColor(data->description->width,
+				  data->description->height,
+				  fmt,
+				  &(data->pixelData[0]));
+
+
+      
+      jderobot::ImageDataPtr data2 = cprxDepth->getImageData();
+      colorspaces::Image::FormatPtr fmt2 =  colorspaces::Image::Format::searchFormat(data2->description->format);
+      if (!fmt2)
+	throw "Format not supported";
+
+      colorspaces::Image imgDepth(data2->description->width,
+				  data2->description->height,
+				  fmt2,
+				  &(data2->pixelData[0]));
+
+      viewer.setDepth(cprxDepth->getImageData());
+      viewer.display(imgColor, imgDepth);
+
+
+
+
+      
+
+    }
+  }catch (const Ice::Exception& ex) {
+    std::cerr << ex << std::endl;
+    status = 1;
+  } catch (const char* msg) {
+    std::cerr << msg << std::endl;
+    status = 1;
+  }
+
+  if (ic)
+    ic->destroy();
+  return status;
+}
+
+

Added: trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.glade
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.glade	                        (rev 0)
+++ trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.glade	2013-08-12 10:53:01 UTC (rev 970)
@@ -0,0 +1,256 @@
+<?xml version="1.0"?>
+<glade-interface>
+  <!-- interface-requires gtk+ 2.6 -->
+  <!-- interface-naming-policy toplevel-contextual -->
+  <widget class="GtkWindow" id="mainwindow">
+    <property name="visible">True</property>
+    <property name="title" translatable="yes">rgbdCalibrator </property>
+    <child>
+      <widget class="GtkFixed" id="fixed1">
+        <property name="visible">True</property>
+        <child>
+          <widget class="GtkLabel" id="fpslabel">
+            <property name="width_request">66</property>
+            <property name="height_request">21</property>
+            <property name="visible">True</property>
+            <property name="justify">fill</property>
+            <property name="wrap">True</property>
+          </widget>
+          <packing>
+            <property name="x">30</property>
+            <property name="y">267</property>
+          </packing>
+        </child>
+        <child>
+          <widget class="GtkImage" id="color_image">
+            <property name="width_request">320</property>
+            <property name="height_request">240</property>
+            <property name="visible">True</property>
+            <property name="stock">gtk-missing-image</property>
+          </widget>
+          <packing>
+            <property name="x">27</property>
+            <property name="y">21</property>
+          </packing>
+        </child>
+        <child>
+          <widget class="GtkImage" id="depth_image">
+            <property name="width_request">320</property>
+            <property name="height_request">240</property>
+            <property name="visible">True</property>
+            <property name="stock">gtk-missing-image</property>
+          </widget>
+          <packing>
+            <property name="x">28</property>
+            <property name="y">306</property>
+          </packing>
+        </child>
+        <child>
+          <widget class="GtkExpander" id="expander1">
+            <property name="width_request">264</property>
+            <property name="height_request">209</property>
+            <property name="visible">True</property>
+            <property name="can_focus">True</property>
+            <property name="border_width">5</property>
+            <child>
+              <widget class="GtkFixed" id="fixed2">
+                <property name="visible">True</property>
+                <child>
+                  <widget class="GtkLabel" id="label2">
+                    <property name="width_request">120</property>
+                    <property name="height_request">25</property>
+                    <property name="visible">True</property>
+                    <property name="label" translatable="yes">Delay pictures:</property>
+                    <property name="wrap">True</property>
+                    <property name="ellipsize">end</property>
+                  </widget>
+                  <packing>
+                    <property name="x">13</property>
+                    <property name="y">53</property>
+                  </packing>
+                </child>
+                <child>
+                  <widget class="GtkEntry" id="et_sleep_photo">
+                    <property name="width_request">30</property>
+                    <property name="height_request">20</property>
+                    <property name="visible">True</property>
+                    <property name="can_focus">True</property>
+                    <property name="invisible_char">&#x25CF;</property>
+                    <property name="text" translatable="yes">3</property>
+                  </widget>
+                  <packing>
+                    <property name="x">171</property>
+                    <property name="y">57</property>
+                  </packing>
+                </child>
+                <child>
+                  <widget class="GtkLabel" id="label5">
+                    <property name="width_request">150</property>
+                    <property name="height_request">25</property>
+                    <property name="visible">True</property>
+                    <property name="label" translatable="yes">BorderSize Width:</property>
+                    <property name="wrap">True</property>
+                  </widget>
+                  <packing>
+                    <property name="x">18</property>
+                    <property name="y">83</property>
+                  </packing>
+                </child>
+                <child>
+                  <widget class="GtkLabel" id="label1">
+                    <property name="width_request">150</property>
+                    <property name="height_request">25</property>
+                    <property name="visible">True</property>
+                    <property name="label" translatable="yes">BorderSize Height:</property>
+                    <property name="wrap">True</property>
+                  </widget>
+                  <packing>
+                    <property name="x">18</property>
+                    <property name="y">112</property>
+                  </packing>
+                </child>
+                <child>
+                  <widget class="GtkEntry" id="bs_width">
+                    <property name="width_request">30</property>
+                    <property name="height_request">20</property>
+                    <property name="visible">True</property>
+                    <property name="can_focus">True</property>
+                    <property name="invisible_char">&#x25CF;</property>
+                    <property name="text" translatable="yes">9</property>
+                  </widget>
+                  <packing>
+                    <property name="x">171</property>
+                    <property name="y">86</property>
+                  </packing>
+                </child>
+                <child>
+                  <widget class="GtkEntry" id="bs_height">
+                    <property name="width_request">30</property>
+                    <property name="height_request">20</property>
+                    <property name="visible">True</property>
+                    <property name="can_focus">True</property>
+                    <property name="invisible_char">&#x25CF;</property>
+                    <property name="text" translatable="yes">6</property>
+                  </widget>
+                  <packing>
+                    <property name="x">171</property>
+                    <property name="y">112</property>
+                  </packing>
+                </child>
+                <child>
+                  <widget class="GtkLabel" id="label3">
+                    <property name="width_request">150</property>
+                    <property name="height_request">25</property>
+                    <property name="visible">True</property>
+                    <property name="label" translatable="yes">Size square</property>
+                    <property name="wrap">True</property>
+                  </widget>
+                  <packing>
+                    <property name="x">18</property>
+                    <property name="y">142</property>
+                  </packing>
+                </child>
+                <child>
+                  <widget class="GtkEntry" id="bs_square_size">
+                    <property name="width_request">30</property>
+                    <property name="height_request">20</property>
+                    <property name="visible">True</property>
+                    <property name="can_focus">True</property>
+                    <property name="invisible_char">&#x25CF;</property>
+                    <property name="text" translatable="yes">20</property>
+                  </widget>
+                  <packing>
+                    <property name="x">171</property>
+                    <property name="y">142</property>
+                  </packing>
+                </child>
+                <child>
+                  <widget class="GtkLabel" id="label4">
+                    <property name="width_request">120</property>
+                    <property name="height_request">25</property>
+                    <property name="visible">True</property>
+                    <property name="label" translatable="yes">Num pictures:</property>
+                    <property name="wrap">True</property>
+                    <property name="ellipsize">end</property>
+                  </widget>
+                  <packing>
+                    <property name="x">11</property>
+                    <property name="y">24</property>
+                  </packing>
+                </child>
+                <child>
+                  <widget class="GtkEntry" id="et_num_photo">
+                    <property name="width_request">30</property>
+                    <property name="height_request">20</property>
+                    <property name="visible">True</property>
+                    <property name="can_focus">True</property>
+                    <property name="invisible_char">&#x25CF;</property>
+                    <property name="text" translatable="yes">8</property>
+                  </widget>
+                  <packing>
+                    <property name="x">171</property>
+                    <property name="y">29</property>
+                  </packing>
+                </child>
+              </widget>
+            </child>
+            <child>
+              <widget class="GtkLabel" id="label6">
+                <property name="visible">True</property>
+                <property name="label" translatable="yes">Intrinsics Calibration Config</property>
+              </widget>
+              <packing>
+                <property name="type">label_item</property>
+              </packing>
+            </child>
+          </widget>
+          <packing>
+            <property name="x">650</property>
+            <property name="y">24</property>
+          </packing>
+        </child>
+        <child>
+          <widget class="GtkButton" id="bt_take_photo">
+            <property name="label" translatable="yes">1. TakePictures</property>
+            <property name="width_request">133</property>
+            <property name="height_request">40</property>
+            <property name="visible">True</property>
+            <property name="can_focus">True</property>
+            <property name="receives_default">True</property>
+          </widget>
+          <packing>
+            <property name="x">435</property>
+            <property name="y">46</property>
+          </packing>
+        </child>
+        <child>
+          <widget class="GtkButton" id="bt_intrinsic_calib">
+            <property name="label" translatable="yes">2. Intrinsic Calibration</property>
+            <property name="width_request">172</property>
+            <property name="height_request">40</property>
+            <property name="visible">True</property>
+            <property name="can_focus">True</property>
+            <property name="receives_default">True</property>
+          </widget>
+          <packing>
+            <property name="x">414</property>
+            <property name="y">94</property>
+          </packing>
+        </child>
+        <child>
+          <widget class="GtkTextView" id="tv_status">
+            <property name="width_request">255</property>
+            <property name="height_request">112</property>
+            <property name="visible">True</property>
+            <property name="can_focus">True</property>
+            <property name="text" translatable="yes">Info</property>
+          </widget>
+          <packing>
+            <property name="x">381</property>
+            <property name="y">147</property>
+          </packing>
+        </child>
+      </widget>
+    </child>
+  </widget>
+</glade-interface>

Added: trunk/src/stable/components/rgbdCalibrator/viewer.cpp
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/viewer.cpp	                        (rev 0)
+++ trunk/src/stable/components/rgbdCalibrator/viewer.cpp	2013-08-12 10:53:01 UTC (rev 970)
@@ -0,0 +1,294 @@
+/*
+ *
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see http://www.gnu.org/licenses/. 
+ *
+ *  Authors : Roberto Calvo <rocapal [at] gsyc [dot] urjc [dot] es>
+ *
+ */
+
+#include "viewer.h" 
+#include <iostream>
+#include <Ice/Ice.h>
+#include <IceUtil/IceUtil.h>
+#include <boost/filesystem.hpp>
+#include "calibration.h"
+
+using namespace boost::filesystem; 
+using namespace cv;
+
+namespace rgbdCalibrator{
+  
+  
+  const std::string gladepath = std::string(GLADE_DIR) + 
+    std::string("/rgbdCalibrator.glade");
+
+  const std::string pathImage = "./images/";
+
+
+  Viewer::Viewer() 
+    : gtkmain(0,0),frameCount(0),
+      intrinsicsEnable(0),contPhoto(1) {
+
+    std::cout << "Loading glade\n";
+
+    // ref widgets
+    refXml = Gnome::Glade::Xml::create(gladepath);
+    refXml->get_widget("color_image", gtkimage_color);
+    refXml->get_widget("depth_image", gtkimage_depth);
+    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);
+
+    // connect signals
+    btTakePhoto->signal_clicked().connect(sigc::mem_fun(this,&Viewer::on_bt_take_photo_clicked));
+
+    btIntrinsic->signal_clicked().connect(sigc::mem_fun(this,&Viewer::on_bt_intrinsic));
+        
+
+    // start the timer for calculating the number of frames per second
+    // the images are being displayed at
+    oldFrameTime = IceUtil::Time::now();
+
+   
+  }
+    
+
+  Viewer::~Viewer() {}
+
+  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);
+
+    if (intrinsicsEnable)
+      saveImage(imageColor);
+    
+
+    colorspaces::ImageRGB8 img_rgb8D(imageDepth);
+    Glib::RefPtr<Gdk::Pixbuf> imgBuffDepth = 
+      Gdk::Pixbuf::create_from_data((const guint8*)img_rgb8D.data,
+				    Gdk::COLORSPACE_RGB,
+				    false,
+				    8,
+				    img_rgb8D.width,
+				    img_rgb8D.height,
+				    img_rgb8D.step);
+    
+    gtkimage_depth->clear();
+    gtkimage_depth->set(imgBuffDepth);
+    
+
+
+    displayFrameRate();
+    mainwindow->resize(1,1);
+    while (gtkmain.events_pending())
+      gtkmain.iteration();
+  }
+    
+  void Viewer::setDepth(const jderobot::ImageDataPtr depth)
+  {
+    dataDepth = depth;
+  }
+
+  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());
+    }
+  }
+
+  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)
+    {
+
+      // Check the directory
+      if ( !exists(pathImage)) 
+      {
+	path dir(pathImage);
+	if (!boost::filesystem::create_directory(dir))
+	  std::cout << "Error to create directory" << std::endl;
+      }
+
+
+      lastTimePhoto = IceUtil::Time::now();
+
+      // Convert to gray
+      Mat gray(imageColor.size(), CV_8UC1);
+      cvtColor(imageColor, gray, CV_RGB2GRAY);
+
+      // 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());
+      
+      beep();
+
+      if (contPhoto < numPhoto)
+	contPhoto ++;
+      else
+	{
+	  intrinsicsEnable = 0;
+	  contPhoto = 1;
+	}	
+    }
+
+  }
+
+  void Viewer::beep()
+  {
+    system("mplayer beep.wav &> /dev/null");
+  }
+  
+  void Viewer::on_bt_intrinsic() 
+  {
+
+
+    tvStatus->get_buffer()->set_text("Getting intrinsics ...");
+
+    Size boardSize, imageSize; 
+    Mat cameraMatrix, distCoeffs;
+    vector<vector<Point2f> > imagePoints;
+
+
+    // TODO: get this data from user by GUI
+    boardSize.width = 9;
+    boardSize.height= 6;
+
+    //flag
+    int flag = 0;
+    flag |= CV_CALIB_FIX_PRINCIPAL_POINT;
+    flag |= CV_CALIB_ZERO_TANGENT_DIST;
+    flag |= CV_CALIB_FIX_ASPECT_RATIO;
+
+    // List of images
+    directory_iterator end_itr; 
+    for ( directory_iterator itr( pathImage ); itr != end_itr; ++itr )
+    {
+
+      Mat view;
+      view = imread(itr->path().c_str(), CV_LOAD_IMAGE_COLOR);
+
+      imageSize = view.size();
+
+      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 ));
+
+	imagePoints.push_back(pointBuf);
+
+	// Draw the corners.
+	drawChessboardCorners( view, boardSize, Mat(pointBuf), found );
+	//imshow("Image View", view);
+	
+      }
+    }
+
+    Calibration* calib = new Calibration();
+    calib->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());
+    }
+
+    calib->extrinsics(cameraMatrix, dataDepth);
+
+  }
+
+
+}//namespace

Added: trunk/src/stable/components/rgbdCalibrator/viewer.h
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/viewer.h	                        (rev 0)
+++ trunk/src/stable/components/rgbdCalibrator/viewer.h	2013-08-12 10:53:01 UTC (rev 970)
@@ -0,0 +1,99 @@
+/*
+ *
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see http://www.gnu.org/licenses/. 
+ *
+ *  Authors : Roberto Calvo <rocapal [at] gsyc [dot] urjc [dot] es>
+ *
+ */
+
+#ifndef RGBDCALIBRATOR_VIEWER_H
+#define RGBDCALIBRATOR_VIEWER_H
+
+
+#include <gtkmm.h>
+#include <libglademm.h>
+#include <IceUtil/Thread.h>
+#include <IceUtil/Time.h>
+#include <string>
+#include <colorspaces/colorspacesmm.h>
+#include <cmath>
+#include <jderobot/camera.h>
+
+using namespace cv;
+
+namespace rgbdCalibrator{
+
+
+  class Viewer
+  {
+  public:
+    Viewer();
+    ~Viewer();
+  
+    bool isVisible();
+
+    //! function that actually displays the image in a window
+    void display( const colorspaces::Image& imageColor, const colorspaces::Image& imageDepth );
+    
+    void setDepth(const jderobot::ImageDataPtr depth);
+
+  private:
+    Glib::RefPtr<Gnome::Glade::Xml> refXml;
+    Gtk::Image* gtkimage_color;
+    Gtk::Image* gtkimage_depth;
+    Gtk::Window* mainwindow;
+    Gtk::Label* fpsLabel;
+    Gtk::Label* lbSleepPhoto;
+    Gtk::Button* btTakePhoto;
+    Gtk::Button* btIntrinsic;
+    Gtk::Entry* etSleepPhoto;
+    Gtk::Entry* etNumPhoto;
+    Gtk::TextView* tvStatus;
+    Gtk::Main gtkmain;
+  
+    //! display the frame rate of the received images
+    void displayFrameRate();
+      
+    //! time variables for calculating number of frames per second 
+    IceUtil::Time currentFrameTime,oldFrameTime;
+    double fps;
+    int frameCount;
+
+    // Intrinsics variables
+    int intrinsicsEnable;
+    IceUtil::Time lastTimePhoto;
+    int delayPhoto;
+    int numPhoto;
+    int contPhoto;
+
+    jderobot::ImageDataPtr dataDepth;
+    
+
+    // onclicks
+    void on_bt_take_photo_clicked ();
+    void on_bt_intrinsic();
+    
+    void saveImage(const colorspaces::Image& imageColor);
+    void beep();
+
+
+    
+  
+  };
+
+}//namespace
+
+#endif //RGBDCALIBRATOR_VIEWER_H



More information about the Jderobot-admin mailing list