[Jderobot-admin] jderobot-r985 - trunk/src/stable/components/rgbdCalibrator

rocapal en jderobot.org rocapal en jderobot.org
Jue Sep 19 17:16:55 CEST 2013


Author: rocapal
Date: 2013-09-19 17:15:55 +0200 (Thu, 19 Sep 2013)
New Revision: 985

Modified:
   trunk/src/stable/components/rgbdCalibrator/CMakeLists.txt
   trunk/src/stable/components/rgbdCalibrator/calibration.cpp
   trunk/src/stable/components/rgbdCalibrator/calibration.h
   trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.glade
   trunk/src/stable/components/rgbdCalibrator/viewer.cpp
   trunk/src/stable/components/rgbdCalibrator/viewer.h
Log:
#27 added least squares optimization


Modified: trunk/src/stable/components/rgbdCalibrator/CMakeLists.txt
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/CMakeLists.txt	2013-09-18 13:25:36 UTC (rev 984)
+++ trunk/src/stable/components/rgbdCalibrator/CMakeLists.txt	2013-09-19 15:15:55 UTC (rev 985)
@@ -24,4 +24,5 @@
     ${LIBS_DIR}/geometry/libgeometry.so
     ${LIBS_DIR}/colorspaces/libcolorspaces.so
     ${LIBS_DIR}/cvBlob/libcvblob.so
+    ${gsl_LIBRARIES}
 )

Modified: trunk/src/stable/components/rgbdCalibrator/calibration.cpp
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/calibration.cpp	2013-09-18 13:25:36 UTC (rev 984)
+++ trunk/src/stable/components/rgbdCalibrator/calibration.cpp	2013-09-19 15:15:55 UTC (rev 985)
@@ -25,6 +25,11 @@
 #include "../../libs/geometry/math/Point3D.h"
 #include "../../libs/geometry/math/Segment3D.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{
@@ -36,7 +41,9 @@
 
 
   Calibration::Calibration()
-  {}
+  {
+    initPatternPoints();
+  }
 
   Calibration::~Calibration()
   {}
@@ -164,312 +171,8 @@
     return std::sqrt(totalErr/totalPoints);
   }
 
-  void Calibration::getOpticalCenter (Eigen::Vector2d &center)
-  {
-    center(0) = mKMatrix.at<double>(0,2);
-    center(1) = mKMatrix.at<double>(1,2);
-  }
 
-  // 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);
-  }
-
-
   void
   Calibration::BackProjectWithDepth (const Eigen::Vector3d pixel,
 				     const colorspaces::Image depthData,
@@ -522,7 +225,7 @@
     Progeo::Progeo* progeo = new Progeo::Progeo(posCamera, K, RT, 320, 240);
     //progeo->display_camerainfo(); 
 
-    std::cout << "Pixel (opticas): " << pixel << std::endl;
+    //std::cout << "Pixel (opticas): " << pixel << std::endl;
    
     // Optical coordinates
     Eigen::Vector3d graphic;
@@ -530,25 +233,25 @@
     graphic(1) = 240 - 1 - pixel(0);
     graphic(2) = 1.0;
 
-    std::cout << "Graphic (progeo): " << graphic << std::endl;
+    //std::cout << "Graphic (progeo): " << graphic << std::endl;
 
     Eigen::Vector4d p3D;
     progeo->backproject(graphic, p3D); 
 
-    std::cout << "P3D: " << p3D << std::endl;
+    //std::cout << "P3D: " << p3D << std::endl;
 
     Point3D *pStart = new Point3D(0.0,0.0,0.0);
     Point3D *pEnd = new Point3D(p3D);
 
     Segment3D *segment = new Segment3D(*pStart,*pEnd);
 
-    std::cout << "Depth: " << depth << std::endl;
+    //std::cout << "Depth: " << depth << std::endl;
 
     Point3D *nP3D = segment->getPointByZ(depth);    
 
     res3D = nP3D->getPoint();
 
-    std::cout << res3D << std::endl;
+    //std::cout << res3D << std::endl;
 
     /*
     std::cout << "-------------" << std::endl;
@@ -579,4 +282,186 @@
 
   }
 
+  void Calibration::initPatternPoints()
+  {
+    mPatternPoints.push_back(Eigen::Vector4d (0.,0.,0.,1.));
+    mPatternPoints.push_back(Eigen::Vector4d (120.,0.,0.,1.));
+    mPatternPoints.push_back(Eigen::Vector4d (0.,0.,120.,1.));
+    mPatternPoints.push_back(Eigen::Vector4d (0.,120.,0.,1.));
+    mPatternPoints.push_back(Eigen::Vector4d (120.,120.,0.,1.));
+    mPatternPoints.push_back(Eigen::Vector4d (0.,120.,120.,1.));
+
+  }
+
+  bool Calibration::addPatternPixel (Eigen::Vector3d pixel, const colorspaces::Image depthData)
+  {
+    mPixelPoints.push_back(pixel);
+    
+    if  (mPixelPoints.size() > 6)
+      return false;
+
+    if (mPixelPoints.size() == 6)
+    {  
+
+      std::cout << "\tPixels" << "\t\t" << "Pattern" << "\t\t" << "Camera" << std::endl;
+
+      for (int i=0; i<mPixelPoints.size(); i++)
+      {
+        
+         std::cout << "P" << i << "\t(" 
+		   << mPixelPoints[i](0) << "," 
+		   << mPixelPoints[i](1) << ")  \t";
+
+         std::cout << "(" << mPatternPoints[i](0) << "," 
+		   << mPatternPoints[i](1) << "," 
+		   << mPatternPoints[i](2) << ") \t";
+
+	 Eigen::Vector4d pCamera(0.,0.,1000000.,0.);
+
+	 // 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);
+	     
+	     if (aux(2) != 0 && aux(2) < pCamera(2)) {
+	       pCamera = aux;
+             }
+           }
+         }
+
+	 mCameraPoints.push_back(pCamera);
+
+	 std::cout << "(" << pCamera[0] 
+		   << "," << pCamera[1] 
+		   << "," << pCamera[2] << ")"
+		   << std::endl;
+
+
+	
+	 
+      }
+
+      mPairPoints.clear();
+      
+      //Build pairs
+      for (int i = 0; i<4; i++)
+	mPairPoints.push_back(std::make_pair(mPatternPoints[i] ,mCameraPoints[i]));
+
+      
+      for (int i = 0; i<4; 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();      
+
+      mCameraPoints.clear();
+      mPairPoints.clear();
+
+    }
+
+    return true;
+
+  }
+
+  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.;
+
+    //gsl_matrix_fprintf(stdout,ertm,"%f");
+
+    std::cout << mRTsolution << std::endl;
+
+    
+
+  }
+
 }

Modified: trunk/src/stable/components/rgbdCalibrator/calibration.h
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/calibration.h	2013-09-18 13:25:36 UTC (rev 984)
+++ trunk/src/stable/components/rgbdCalibrator/calibration.h	2013-09-19 15:15:55 UTC (rev 985)
@@ -31,6 +31,7 @@
 #include <colorspaces/colorspacesmm.h>
 #include <jderobot/camera.h>
 #include "../../libs/geometry/math/Point3D.h"
+#include <boost/tuple/tuple.hpp>
 
 using namespace cv;
 
@@ -55,17 +56,33 @@
 			       Mat& distCoeffs,
 			       vector<vector<Point2f> > imagePoints );
 
-    void extrinsics (const Mat& kMatrix, const jderobot::ImageDataPtr depth);
-
     void BackProjectWithDepth (const Eigen::Vector3d pixel,
 			       const colorspaces::Image depthData,
 			       Eigen::Vector4d& res3D);
 
-    void getOpticalCenter (Eigen::Vector2d &center);
+    bool addPatternPixel (Eigen::Vector3d pixel, 
+			  const colorspaces::Image depthData);
+
+    Eigen::Matrix4d getRTSolution() { return mRTsolution; };
+
+    void test(Eigen::Vector4d pCamera);
+
   private:
 
     Mat mKMatrix;
 
+    Eigen::Matrix4d mRTsolution;
+
+    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 initPatternPoints();
+
+    void LSO();
+   
+
     void calcBoardCornerPositions(Size boardSize, 
 				  float squareSize, 
 				  vector<Point3f>& corners,  

Modified: trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.glade
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.glade	2013-09-18 13:25:36 UTC (rev 984)
+++ trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.glade	2013-09-19 15:15:55 UTC (rev 985)
@@ -281,6 +281,25 @@
             <property name="y">306</property>
           </packing>
         </child>
+        <child>
+          <widget class="GtkEventBox" id="eb_extrinsics">
+            <property name="width_request">320</property>
+            <property name="height_request">240</property>
+            <property name="visible">True</property>
+            <child>
+              <widget class="GtkImage" id="color_image2">
+                <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>
+            </child>
+          </widget>
+          <packing>
+            <property name="x">28</property>
+            <property name="y">577</property>
+          </packing>
+        </child>
       </widget>
     </child>
   </widget>

Modified: trunk/src/stable/components/rgbdCalibrator/viewer.cpp
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/viewer.cpp	2013-09-18 13:25:36 UTC (rev 984)
+++ trunk/src/stable/components/rgbdCalibrator/viewer.cpp	2013-09-19 15:15:55 UTC (rev 985)
@@ -43,13 +43,14 @@
 
   Viewer::Viewer() 
     : gtkmain(0,0),frameCount(0),
-      intrinsicsEnable(0),contPhoto(1),hsvFilter(NULL) {
+      intrinsicsEnable(0),contPhoto(1),hsvFilter(NULL), mFrameBlob(NULL) {
 
     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);
@@ -61,13 +62,16 @@
     refXml->get_widget("tv_status", tvStatus);
     refXml->get_widget("bt_intrinsic_calib", btIntrinsic);
     refXml->get_widget("eventbox", ebImage);
+    refXml->get_widget("eb_extrinsics", ebImageExtrinsics);
 
     // 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));
-    ebImage->signal_button_press_event().connect(sigc::mem_fun(this, &Viewer::on_eventbox_clicked));   
+    ebImage->signal_button_press_event().connect(sigc::mem_fun(this, &Viewer::on_eventbox_clicked));  
 
+    ebImageExtrinsics->signal_button_press_event().connect(sigc::mem_fun(this, &Viewer::on_eventbox_extrinsics_clicked)); 
+
     // start the timer for calculating the number of frames per second
     // the images are being displayed at
     oldFrameTime = IceUtil::Time::now();
@@ -85,7 +89,9 @@
 
   Viewer::~Viewer() 
   {
-    delete(mCalibration);
+    if(mCalibration)
+      delete(mCalibration);
+
     RGB2HSV_destroyTable();
   }
 
@@ -109,6 +115,9 @@
     gtkimage_color->clear();
     gtkimage_color->set(imgBuffColor);
 
+    gtkimage_color2->clear();
+    gtkimage_color2->set(imgBuffColor);
+
     if (intrinsicsEnable)
       saveImage(imageColor);
     
@@ -116,9 +125,34 @@
     pthread_mutex_lock(&mutex);
     imgOrig.create(imageColor.size(), CV_8UC3);
     imageColor.copyTo(imgOrig);
+
+    mImageDepth = imageDepth.clone();
     pthread_mutex_unlock(&mutex);
     
-    
+    // Show depth image          
+    /*
+    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);
+    */
+    Glib::RefPtr<Gdk::Pixbuf> imgBuffDepth = 
+      Gdk::Pixbuf::create_from_data((const guint8*)imageDepth.data,
+				    Gdk::COLORSPACE_RGB,
+				    false,
+				    8,
+				    imageDepth.width,
+				    imageDepth.height,
+				    imageDepth.step);
+
+    gtkimage_depth->clear();
+    gtkimage_depth->set(imgBuffDepth);
+
     if (hsvFilter != NULL)
     {
       createImageHSV(imageDepth);
@@ -150,27 +184,9 @@
 
       gtkimage_blob->clear();
       gtkimage_blob->set(imgBuffBLOB);
-
-      cvReleaseImage(&mFrameBlob);
-
-      // Show depth image          
-      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);
-
-
+      //cvReleaseImage(&mFrameBlob);
       
-      
-
     }
 
     displayFrameRate();
@@ -215,7 +231,11 @@
     CvBlobs blobs;
 
     IplImage *iplOrig = new IplImage(imgOrig);
+
+    if (mFrameBlob)
+      cvReleaseImage(&mFrameBlob);
     mFrameBlob=cvCreateImage(imgOrig.size(),8,3);
+
     IplImage *labelImg=cvCreateImage(imgOrig.size(),IPL_DEPTH_LABEL,1);
 
     cvResize(iplOrig,mFrameBlob,CV_INTER_LINEAR );
@@ -233,8 +253,8 @@
     cvFilterByArea(blobs,500,5000);    
 
     double area = 0.0;
-    int x, y;
-
+    int x=0;
+    int y=0;
     
     for (CvBlobs::const_iterator it=blobs.begin(); it!=blobs.end(); ++it)
     {
@@ -267,21 +287,13 @@
       
       mCalibration->BackProjectWithDepth(pixel, imageDepth, target);
 
-      //Eigen::Vector2d center;
-      //mCalibration->getOpticalCenter(center);
-      //std::cout << "Center: " << center << std::endl;
-      
     }
 
     // Release and free memory
     delete(iplOrig);
     cvReleaseImage(&threshy);
     cvReleaseImage(&labelImg);
-  }
 
-  void Viewer::setDepth(const jderobot::ImageDataPtr depth)
-  {
-    dataDepth = depth;
   }
 
   void
@@ -305,56 +317,79 @@
     }
   }
 
- bool Viewer::on_eventbox_clicked(GdkEventButton * event)
- {
-   int posX, posY;
-   float r,g,b;
-   posX = (int) event->x;
-   posY = (int) event->y;
+  bool Viewer::on_eventbox_extrinsics_clicked(GdkEventButton * event)
+  {
 
-   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 (mCalibration){
    
-   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;
+      pthread_mutex_lock(&mutex);
+      bool res = mCalibration->addPatternPixel (Eigen::Vector3d ((int) event->x, 
+								 (int) event->y, 
+								 1.0),
+						mImageDepth);
 
-   // 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;
+      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);
+      }
 
-   smax = hsvFilter->S + 0.1;
-   smin = hsvFilter->S - 0.1;
-   if(smax > 1.0)
-     smax = 1.0;
-   if(smin < 0.0)
-     smin = 0.0;
+      pthread_mutex_unlock(&mutex);
+    }
+  }
 
-   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]: " <<
+  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;
- }
-
+	"[" << smin << " " << smax << "] " <<
+	"[" << vmin << " " << vmax << "] " << std::endl;
+    
+    return true;
+  }
+  
   void Viewer::on_bt_take_photo_clicked() 
   {
     intrinsicsEnable = 1;
@@ -499,8 +534,6 @@
       tvStatus->get_buffer()->set_text(matrixStr.str().c_str());
     }
 
-    mCalibration->extrinsics(cameraMatrix, dataDepth);
-
     
   }
 

Modified: trunk/src/stable/components/rgbdCalibrator/viewer.h
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/viewer.h	2013-09-18 13:25:36 UTC (rev 984)
+++ trunk/src/stable/components/rgbdCalibrator/viewer.h	2013-09-19 15:15:55 UTC (rev 985)
@@ -55,6 +55,7 @@
   private:
     Glib::RefPtr<Gnome::Glade::Xml> refXml;
     Gtk::Image* gtkimage_color;
+    Gtk::Image* gtkimage_color2;
     Gtk::Image* gtkimage_depth;
     Gtk::Image* gtkimage_hsv;
     Gtk::Image* gtkimage_blob;
@@ -67,6 +68,7 @@
     Gtk::Entry* etNumPhoto;
     Gtk::TextView* tvStatus;
     Gtk::EventBox* ebImage;
+    Gtk::EventBox* ebImageExtrinsics;
     Gtk::Main gtkmain;
   
     //! display the frame rate of the received images
@@ -85,16 +87,19 @@
     int numPhoto;
     int contPhoto;
 
-    jderobot::ImageDataPtr dataDepth;
     cv::Mat imgOrig;
     cv::Mat imgHSV;
+    colorspaces::Image mImageDepth;
     IplImage* mFrameBlob;
     pthread_mutex_t mutex;
     const HSV* hsvFilter;
     double hmin, hmax, smin, smax, vmin, vmax;
 
+    // Extrinsics variables
+
     // onclicks
     bool on_eventbox_clicked(GdkEventButton * event);
+    bool on_eventbox_extrinsics_clicked(GdkEventButton * event);
     void on_bt_take_photo_clicked ();
     void on_bt_intrinsic();
     



More information about the Jderobot-admin mailing list