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

rocapal en jderobot.org rocapal en jderobot.org
Mie Ago 28 11:08:37 CEST 2013


Author: rocapal
Date: 2013-08-28 11:07:37 +0200 (Wed, 28 Aug 2013)
New Revision: 976

Modified:
   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/rgbdCalibrator.glade
   trunk/src/stable/components/rgbdCalibrator/viewer.cpp
   trunk/src/stable/components/rgbdCalibrator/viewer.h
Log:
[27] added depth and blob images


Modified: trunk/src/stable/components/rgbdCalibrator/calibration.cpp
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/calibration.cpp	2013-08-27 09:22:05 UTC (rev 975)
+++ trunk/src/stable/components/rgbdCalibrator/calibration.cpp	2013-08-28 09:07:37 UTC (rev 976)
@@ -21,7 +21,6 @@
 
 #include <calibration.h>
 #include <iostream>
-#include "geoUtils.h"
 #include "../../libs/geometry/progeo/Progeo.h"
 #include "../../libs/geometry/math/Point3D.h"
 #include "../../libs/geometry/math/Segment3D.h"
@@ -39,6 +38,8 @@
   Calibration::Calibration()
   {}
 
+  Calibration::~Calibration()
+  {}
 
   // Intrinsics 
   bool Calibration::runCalibrationAndSave(Size &boardSize, 
@@ -60,7 +61,9 @@
     std::cout << (ok ? "Calibration succeeded" : "Calibration failed")
 	 << ". avg re projection error = "  << totalAvgErr ;
 
+    mKMatrix = Mat(cameraMatrix);
 
+
     return ok;
   }
  
@@ -461,4 +464,81 @@
     delete(progeo);
   }
 
+
+  void
+  Calibration::BackProjectWithDepth (const Eigen::Vector3d pixel,
+				     const jderobot::ImageDataPtr depthData,
+				     Eigen::Vector4d& res3D)
+  {
+
+
+    Eigen::Vector4d posCamera;
+    posCamera(0) = double(0.0);
+    posCamera(1) = double(0.0);
+    posCamera(2) = double(0.0);
+
+
+    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);
+
+    K(1,0) = mKMatrix.at<double>(1,0);
+    K(1,1) = mKMatrix.at<double>(1,1);
+    K(1,2) = mKMatrix.at<double>(1,2);
+
+    K(2,0) = mKMatrix.at<double>(2,0);
+    K(2,1) = mKMatrix.at<double>(2,1);
+    K(2,2) = mKMatrix.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;
+    progeo->backproject(pixel, p3D); 
+
+    Point3D *pStart = new Point3D(0.0,0.0,0.0);
+    Point3D *pEnd = new Point3D(p3D);
+
+    Segment3D *segment = new Segment3D(*pStart,*pEnd);
+
+    float depth = (int)depthData->pixelData[((depthData->description->width*(int)pixel(1))+(int)pixel(0))*3+1]<<8 | (int)depthData->pixelData[((depthData->description->width*(int)pixel(1))+(int)pixel(0))*3+2];
+
+    Point3D *nP3D = segment->getPointByZ(depth);    
+
+    //Eigen::Vector4d* resP3D = new Eigen::Vector4d(nP3D->getPoint());
+
+    res3D = nP3D->getPoint();
+
+    delete(segment);
+    delete(pStart);
+    delete(pEnd);
+    delete(nP3D);
+    delete(progeo);
+
+  }
+
 }

Modified: trunk/src/stable/components/rgbdCalibrator/calibration.h
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/calibration.h	2013-08-27 09:22:05 UTC (rev 975)
+++ trunk/src/stable/components/rgbdCalibrator/calibration.h	2013-08-28 09:07:37 UTC (rev 976)
@@ -30,6 +30,7 @@
 #include <string>
 #include <colorspaces/colorspacesmm.h>
 #include <jderobot/camera.h>
+#include "../../libs/geometry/math/Point3D.h"
 
 using namespace cv;
 
@@ -58,6 +59,7 @@
     
   private:
 
+    Mat mKMatrix;
 
     void calcBoardCornerPositions(Size boardSize, 
 				  float squareSize, 
@@ -83,10 +85,17 @@
 			vector<Mat>& tvecs,
 			vector<float>& reprojErrs,  
 			double& totalAvgErr);
+
+
+    void BackProjectWithDepth (const Eigen::Vector3d pixel,
+			       const jderobot::ImageDataPtr depthData,
+			       Eigen::Vector4d& res3D);
     
   };
 
+  
 
+
 }
 
 

Modified: trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.cpp
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.cpp	2013-08-27 09:22:05 UTC (rev 975)
+++ trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.cpp	2013-08-28 09:07:37 UTC (rev 976)
@@ -70,7 +70,7 @@
 				  &(data->pixelData[0]));
 
 
-      /*
+      
       jderobot::ImageDataPtr data2 = cprxDepth->getImageData();
       colorspaces::Image::FormatPtr fmt2 =  colorspaces::Image::Format::searchFormat(data2->description->format);
       if (!fmt2)
@@ -80,12 +80,12 @@
 				  data2->description->height,
 				  fmt2,
 				  &(data2->pixelData[0]));
-      */
+      
 
       //viewer.setDepth(NULL);//cprxDepth->getImageData());
 
 
-      viewer.display(imgColor, imgColor);
+      viewer.display(imgColor, imgDepth);
 
 
 

Modified: trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.glade
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.glade	2013-08-27 09:22:05 UTC (rev 975)
+++ trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.glade	2013-08-28 09:07:37 UTC (rev 976)
@@ -257,6 +257,30 @@
             <property name="y">25</property>
           </packing>
         </child>
+        <child>
+          <widget class="GtkImage" id="hsv_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">358</property>
+            <property name="y">306</property>
+          </packing>
+        </child>
+        <child>
+          <widget class="GtkImage" id="blob_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">688</property>
+            <property name="y">306</property>
+          </packing>
+        </child>
       </widget>
     </child>
   </widget>

Modified: trunk/src/stable/components/rgbdCalibrator/viewer.cpp
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/viewer.cpp	2013-08-27 09:22:05 UTC (rev 975)
+++ trunk/src/stable/components/rgbdCalibrator/viewer.cpp	2013-08-28 09:07:37 UTC (rev 976)
@@ -24,7 +24,6 @@
 #include <Ice/Ice.h>
 #include <IceUtil/IceUtil.h>
 #include <boost/filesystem.hpp>
-#include "calibration.h"
 #include "../../libs/cvBlob/cvblob.h"
 
 #define DEGTORAD     (3.14159264 / 180.0)
@@ -53,6 +52,8 @@
     refXml = Gnome::Glade::Xml::create(gladepath);
     refXml->get_widget("color_image", gtkimage_color);
     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);
@@ -74,6 +75,9 @@
 
     RGB2HSV_init();
     RGB2HSV_createTable();
+
+    mCalibration = new Calibration();
+
     pthread_mutex_init(&mutex, NULL);
     
 
@@ -82,6 +86,7 @@
 
   Viewer::~Viewer() 
   {
+    delete(mCalibration);
     RGB2HSV_destroyTable();
   }
 
@@ -118,10 +123,9 @@
     if (hsvFilter != NULL)
     {
       createImageHSV();
-    
-          
-      //colorspaces::ImageRGB8 img_rgb8D(imgHSV);
-      Glib::RefPtr<Gdk::Pixbuf> imgBuffDepth = 
+
+      // Show HSV image
+      Glib::RefPtr<Gdk::Pixbuf> imgBuffHSV = 
 	Gdk::Pixbuf::create_from_data((const guint8*)imgHSV.data,
 				      Gdk::COLORSPACE_RGB,
 				      false,
@@ -130,8 +134,48 @@
 				      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);
+
+      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);
+
+      Eigen::Vector3d pixel;
+      pixel(0) = 2;
+      pixel(1) = 3;
+      pixel(2) = 1.;
+
+      Eigen::Vector4d target;
+
+      //mCalibration->BackProjectWithDepth(pixel, imageDepth, &target);
       
 
     }
@@ -178,10 +222,10 @@
     CvBlobs blobs;
 
     IplImage *iplOrig = new IplImage(imgOrig);
-    IplImage *frame=cvCreateImage(imgOrig.size(),8,3);
+    mFrameBlob=cvCreateImage(imgOrig.size(),8,3);
     IplImage *labelImg=cvCreateImage(imgOrig.size(),IPL_DEPTH_LABEL,1);
 
-    cvResize(iplOrig,frame,CV_INTER_LINEAR );
+    cvResize(iplOrig,mFrameBlob,CV_INTER_LINEAR );
 
     //Threshy is a binary image
     cvSmooth(threshy,threshy,CV_MEDIAN,7,7);
@@ -190,26 +234,39 @@
     unsigned int result=cvLabel(threshy,labelImg,blobs);
 
     //Rendering the blobs
-    cvRenderBlobs(labelImg,blobs,frame,frame);
+    cvRenderBlobs(labelImg,blobs,mFrameBlob,mFrameBlob);
 
     //Filter Blobs
-    cvFilterByArea(blobs,300,1000);    
+    cvFilterByArea(blobs,1000,2000);    
 
+    double area = 0.0;
+    int x, y;
+
     for (CvBlobs::const_iterator it=blobs.begin(); it!=blobs.end(); ++it)
     {
-      std::cout << "BLOB found: " << it->second->area  <<std::endl;
+      //std::cout << "BLOB found: " << it->second->area  <<std::endl;
+
       double moment10 = it->second->m10;
       double moment01 = it->second->m01;
-      double area = it->second->area;
+
+      if (it->second->area >= area)
+      {      
+	area = it->second->area;
+	x = moment10/area;
+	y = moment01/area;
+      }
       
     }
 
-    cvShowImage("Live",frame);
+    std::cout << "Max BLOB: " << area << ": " << x << " , " << y  <<std::endl;
 
+    //cvShowImage("Live",mFrameBlob);
+
+    
+
     // Release and free memory
     delete(iplOrig);
     cvReleaseImage(&threshy);
-    cvReleaseImage(&frame);
     cvReleaseImage(&labelImg);
   }
 
@@ -409,10 +466,10 @@
       }
     }
 
-    Calibration* calib = new Calibration();
-    calib->runCalibrationAndSave(boardSize, 20.0, flag, imageSize,  
-				 cameraMatrix, distCoeffs, imagePoints);
 
+    mCalibration->runCalibrationAndSave(boardSize, 20.0, flag, imageSize,  
+					cameraMatrix, distCoeffs, imagePoints);
+
     std::cout << std::endl << cameraMatrix << std::endl;
 
     std::stringstream matrixStr;
@@ -433,8 +490,9 @@
       tvStatus->get_buffer()->set_text(matrixStr.str().c_str());
     }
 
-    calib->extrinsics(cameraMatrix, dataDepth);
+    mCalibration->extrinsics(cameraMatrix, dataDepth);
 
+    
   }
 
 

Modified: trunk/src/stable/components/rgbdCalibrator/viewer.h
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/viewer.h	2013-08-27 09:22:05 UTC (rev 975)
+++ trunk/src/stable/components/rgbdCalibrator/viewer.h	2013-08-28 09:07:37 UTC (rev 976)
@@ -32,6 +32,7 @@
 #include <cmath>
 #include <jderobot/camera.h>
 #include <colorspaces/colorspaces.h>
+#include "calibration.h"
 
 using namespace cv;
 
@@ -55,6 +56,8 @@
     Glib::RefPtr<Gnome::Glade::Xml> refXml;
     Gtk::Image* gtkimage_color;
     Gtk::Image* gtkimage_depth;
+    Gtk::Image* gtkimage_hsv;
+    Gtk::Image* gtkimage_blob;
     Gtk::Window* mainwindow;
     Gtk::Label* fpsLabel;
     Gtk::Label* lbSleepPhoto;
@@ -75,6 +78,7 @@
     int frameCount;
 
     // Intrinsics variables
+    Calibration* mCalibration;
     int intrinsicsEnable;
     IceUtil::Time lastTimePhoto;
     int delayPhoto;
@@ -84,6 +88,7 @@
     jderobot::ImageDataPtr dataDepth;
     cv::Mat imgOrig;
     cv::Mat imgHSV;
+    IplImage* mFrameBlob;
     pthread_mutex_t mutex;
     const HSV* hsvFilter;
     double hmin, hmax, smin, smax, vmin, vmax;



More information about the Jderobot-admin mailing list