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

[rocapal en jderobot.org]

Author: rocapal
Date: 2013-08-30 10:18:30 +0200 (Fri, 30 Aug 2013)
New Revision: 977

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/viewer.cpp
   trunk/src/stable/components/rgbdCalibrator/viewer.h
Log:
#27 extrinsic calibration works based in the origin of camera


Modified: trunk/src/stable/components/rgbdCalibrator/calibration.cpp
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/calibration.cpp	2013-08-28 09:07:37 UTC (rev 976)
+++ trunk/src/stable/components/rgbdCalibrator/calibration.cpp	2013-08-30 08:18:30 UTC (rev 977)
@@ -467,7 +467,7 @@
 
   void
   Calibration::BackProjectWithDepth (const Eigen::Vector3d pixel,
-				     const jderobot::ImageDataPtr depthData,
+				     const colorspaces::Image depthData,
 				     Eigen::Vector4d& res3D)
   {
 
@@ -516,23 +516,29 @@
     Progeo::Progeo* progeo = new Progeo::Progeo(posCamera, K, RT, 320, 240);
     //progeo->display_camerainfo(); 
 
+    std::cout << "Pixel: " << pixel << std::endl;
 
     Eigen::Vector4d p3D;
     progeo->backproject(pixel, p3D); 
 
+    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);
 
-    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];
+    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];
 
+
+    std::cout << "Depth: " << depth << std::endl;
+
     Point3D *nP3D = segment->getPointByZ(depth);    
 
-    //Eigen::Vector4d* resP3D = new Eigen::Vector4d(nP3D->getPoint());
-
     res3D = nP3D->getPoint();
 
+    std::cout << res3D << std::endl;
+
     delete(segment);
     delete(pStart);
     delete(pEnd);

Modified: trunk/src/stable/components/rgbdCalibrator/calibration.h
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/calibration.h	2013-08-28 09:07:37 UTC (rev 976)
+++ trunk/src/stable/components/rgbdCalibrator/calibration.h	2013-08-30 08:18:30 UTC (rev 977)
@@ -56,7 +56,10 @@
 			       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);
   private:
 
     Mat mKMatrix;
@@ -87,9 +90,7 @@
 			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-28 09:07:37 UTC (rev 976)
+++ trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.cpp	2013-08-30 08:18:30 UTC (rev 977)
@@ -82,17 +82,20 @@
 				  &(data2->pixelData[0]));
       
 
-      //viewer.setDepth(NULL);//cprxDepth->getImageData());
+      /*
+      float depth = (int)data2->pixelData[((data2->description->width*(int)120)+(int)160)*3+1]<<8 | (int)data2->pixelData[((data2->description->width*(int)120)+(int)160)*3+2];
+      
+      std::cout << "- Depth (160,120) = " << depth << std::endl;
 
+      depth = (int)imgDepth.data[((imgDepth.cols*(int)120)+(int)160)*3+1]<<8 | (int)imgDepth.data[((imgDepth.cols*(int)120)+(int)160)*3+2];
 
-      viewer.display(imgColor, imgDepth);
+      std::cout << "* Depth (160,120) = " << depth << std::endl;
+      */
 
 
+      viewer.display(imgColor, imgDepth);
+    }
 
-
-      
-
-    }
   }catch (const Ice::Exception& ex) {
     std::cerr << ex << std::endl;
     status = 1;

Modified: trunk/src/stable/components/rgbdCalibrator/viewer.cpp
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/viewer.cpp	2013-08-28 09:07:37 UTC (rev 976)
+++ trunk/src/stable/components/rgbdCalibrator/viewer.cpp	2013-08-30 08:18:30 UTC (rev 977)
@@ -122,7 +122,7 @@
     
     if (hsvFilter != NULL)
     {
-      createImageHSV();
+      createImageHSV(imageDepth);
 
       // Show HSV image
       Glib::RefPtr<Gdk::Pixbuf> imgBuffHSV = 
@@ -168,15 +168,9 @@
       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);
       
+      
 
     }
 
@@ -186,7 +180,7 @@
       gtkmain.iteration();
   }
     
-  void Viewer::createImageHSV()
+  void Viewer::createImageHSV(const colorspaces::Image& imageDepth)
   {
     float r,g,b;
 
@@ -237,7 +231,7 @@
     cvRenderBlobs(labelImg,blobs,mFrameBlob,mFrameBlob);
 
     //Filter Blobs
-    cvFilterByArea(blobs,1000,2000);    
+    cvFilterByArea(blobs,500,2000);    
 
     double area = 0.0;
     int x, y;
@@ -262,7 +256,17 @@
 
     //cvShowImage("Live",mFrameBlob);
 
-    
+    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);

Modified: trunk/src/stable/components/rgbdCalibrator/viewer.h
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/viewer.h	2013-08-28 09:07:37 UTC (rev 976)
+++ trunk/src/stable/components/rgbdCalibrator/viewer.h	2013-08-30 08:18:30 UTC (rev 977)
@@ -100,7 +100,7 @@
     
     void saveImage(const colorspaces::Image& imageColor);
     void beep();
-    void createImageHSV();
+    void createImageHSV(const colorspaces::Image& imageDepth);