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

rocapal en jderobot.org rocapal en jderobot.org
Mie Oct 23 10:21:29 CEST 2013


Author: rocapal
Date: 2013-10-23 10:21:29 +0200 (Wed, 23 Oct 2013)
New Revision: 1073

Added:
   trunk/src/stable/components/rgbdCalibrator/images/
   trunk/src/stable/components/rgbdCalibrator/images/img1.jpg
   trunk/src/stable/components/rgbdCalibrator/images/img2.jpg
   trunk/src/stable/components/rgbdCalibrator/images/img3.jpg
   trunk/src/stable/components/rgbdCalibrator/images/img4.jpg
   trunk/src/stable/components/rgbdCalibrator/images/img5.jpg
   trunk/src/stable/components/rgbdCalibrator/images/img6.jpg
   trunk/src/stable/components/rgbdCalibrator/images/img7.jpg
   trunk/src/stable/components/rgbdCalibrator/images/img8.jpg
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 added images of xtion to intrinsic calibration


Modified: trunk/src/stable/components/rgbdCalibrator/calibration.cpp
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/calibration.cpp	2013-10-22 17:16:24 UTC (rev 1072)
+++ trunk/src/stable/components/rgbdCalibrator/calibration.cpp	2013-10-23 08:21:29 UTC (rev 1073)
@@ -249,6 +249,7 @@
 	mProgeo->saveToFile("./CameraA.xml");
 	*/
 
+	/*
 	mProgeo = new Progeo::Progeo("./cameraB.xml");
 	mProgeo->updateKMatrix();
 	mProgeo->updateRTMatrix();
@@ -269,6 +270,7 @@
 
 
 	progeo_old_test();
+	*/
 }
 
 
@@ -365,8 +367,56 @@
 	 */
 }
 
-bool Calibration::addPatternPixel (Eigen::Vector3d pixel, const colorspaces::Image depthData)
+void Calibration::getBestDepth (const Eigen::Vector3d pixel, const std::vector<colorspaces::Image> depthVector,
+		colorspaces::Image& depthData, Eigen::Vector4d& res3D, Eigen::Vector3d& bestPixel)
 {
+
+	const int WINDOW_SIZE = 5;
+	res3D(2) = 100000.0;
+
+
+	for (std::vector<colorspaces::Image>::iterator it; it != depthVector.end(); ++it)
+	{
+
+		// We use a window of 5x5 pixels to avoid errors of the depth image
+		for (int x=-(WINDOW_SIZE/2); x<=(WINDOW_SIZE/2); x++) {
+			for (int y=-(WINDOW_SIZE/2);y<=(WINDOW_SIZE/2); y++) {
+				int px, py;
+				if (pixel(0)+x < 0)
+					px = 0;
+				else if (pixel(0)+x >= mProgeo->getImageWidth())
+					px = mProgeo->getImageWidth()-1;
+				else
+					px = pixel(0)+x;
+
+				if (pixel(1)+y < 0)
+					py = 0;
+				else if (pixel(1)+y >= mProgeo->getImageHeight())
+					py = mProgeo->getImageHeight()-1;
+				else
+					py =pixel(1)+y;
+
+				Eigen::Vector3d pixel(px,py,1.0);
+				Eigen::Vector4d aux;
+				getRealPoint(pixel, (colorspaces::Image)(*it), aux);
+
+				if (aux(2) != 0 && aux(2) < res3D(2)) {
+					res3D = aux;
+					bestPixel = pixel;
+					depthData = *it;
+				}
+			}
+		}
+
+	}
+
+
+
+}
+
+
+bool Calibration::addPatternPixel (Eigen::Vector3d pixel, const std::vector<colorspaces::Image> depthVector)
+{
 	mPixelPoints.push_back(pixel);
 
 	if  (mPixelPoints.size() > mPatternPoints.size())
@@ -379,6 +429,8 @@
 				  "Transformado Real\t\t" << "Pattern"
 				  <<std::endl;
 
+		const colorspaces::Image depthData;
+
 		for (int i=0; i<mPixelPoints.size(); i++)
 		{
 

Modified: trunk/src/stable/components/rgbdCalibrator/calibration.h
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/calibration.h	2013-10-22 17:16:24 UTC (rev 1072)
+++ trunk/src/stable/components/rgbdCalibrator/calibration.h	2013-10-23 08:21:29 UTC (rev 1073)
@@ -74,7 +74,7 @@
 			       Eigen::Vector4d& res3D);
 
     bool addPatternPixel (Eigen::Vector3d pixel, 
-			  const colorspaces::Image depthData);
+			  const std::vector<colorspaces::Image> depthVector);
 
     Eigen::Matrix4d getRTSolution() { return mRTsolution; };
 
@@ -95,6 +95,9 @@
 
     void initProgeo();
 
+    void getBestDepth (const Eigen::Vector3d pixel, const std::vector<colorspaces::Image> depthVector,
+    		colorspaces::Image& depthData, Eigen::Vector4d& res3D, Eigen::Vector3d& bestPixel);
+
     void getRealPoint(const Eigen::Vector3d pixel,
 		       const colorspaces::Image depthData,
 		       Eigen::Vector4d& res3D);

Added: trunk/src/stable/components/rgbdCalibrator/images/img1.jpg
===================================================================
(Binary files differ)


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

Added: trunk/src/stable/components/rgbdCalibrator/images/img2.jpg
===================================================================
(Binary files differ)


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

Added: trunk/src/stable/components/rgbdCalibrator/images/img3.jpg
===================================================================
(Binary files differ)


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

Added: trunk/src/stable/components/rgbdCalibrator/images/img4.jpg
===================================================================
(Binary files differ)


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

Added: trunk/src/stable/components/rgbdCalibrator/images/img5.jpg
===================================================================
(Binary files differ)


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

Added: trunk/src/stable/components/rgbdCalibrator/images/img6.jpg
===================================================================
(Binary files differ)


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

Added: trunk/src/stable/components/rgbdCalibrator/images/img7.jpg
===================================================================
(Binary files differ)


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

Added: trunk/src/stable/components/rgbdCalibrator/images/img8.jpg
===================================================================
(Binary files differ)


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

Modified: trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.cpp
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.cpp	2013-10-22 17:16:24 UTC (rev 1072)
+++ trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.cpp	2013-10-23 08:21:29 UTC (rev 1073)
@@ -31,82 +31,71 @@
 
 
 int main(int argc, char** argv){
-  int status;
-  rgbdCalibrator::Viewer viewer;
-  Ice::CommunicatorPtr ic;
+	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";
+	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";
+		/*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";
+		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";
+		/*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";
+		while(viewer.isVisible()){
 
-      colorspaces::Image imgColor(data->description->width,
-				  data->description->height,
-				  fmt,
-				  &(data->pixelData[0]));
 
+			jderobot::ImageDataPtr data = cprxColor->getImageData();
+			colorspaces::Image::FormatPtr fmt = colorspaces::Image::Format::searchFormat(data->description->format);
+			if (!fmt)
+				throw "Format not supported";
 
-      
-      jderobot::ImageDataPtr data2 = cprxDepth->getImageData();
-      colorspaces::Image::FormatPtr fmt2 =  colorspaces::Image::Format::searchFormat(data2->description->format);
-      if (!fmt2)
-	throw "Format not supported";
+			colorspaces::Image imgColor(data->description->width,
+					data->description->height,
+					fmt,
+					&(data->pixelData[0]));
 
-      colorspaces::Image imgDepth(data2->description->width,
-				  data2->description->height,
-				  fmt2,
-				  &(data2->pixelData[0]));
-      
 
-      /*
-      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];
+			jderobot::ImageDataPtr data2 = cprxDepth->getImageData();
+			colorspaces::Image::FormatPtr fmt2 =  colorspaces::Image::Format::searchFormat(data2->description->format);
+			if (!fmt2)
+				throw "Format not supported";
 
-      std::cout << "* Depth (160,120) = " << depth << std::endl;
-      */
+			colorspaces::Image imgDepth(data2->description->width,
+					data2->description->height,
+					fmt2,
+					&(data2->pixelData[0]));
 
 
-      viewer.display(imgColor, imgDepth);
-    }
+			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;
-  }
+	}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;
+	if (ic)
+		ic->destroy();
+	return status;
 }
 
 

Modified: trunk/src/stable/components/rgbdCalibrator/viewer.cpp
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/viewer.cpp	2013-10-22 17:16:24 UTC (rev 1072)
+++ trunk/src/stable/components/rgbdCalibrator/viewer.cpp	2013-10-23 08:21:29 UTC (rev 1073)
@@ -43,7 +43,7 @@
 
 Viewer::Viewer()
 : gtkmain(0,0),frameCount(0),
-  intrinsicsEnable(0),contPhoto(1),hsvFilter(NULL), mFrameBlob(NULL) {
+  intrinsicsEnable(0),contPhoto(1),hsvFilter(NULL), mFrameBlob(NULL), MAX_MAPS(25) {
 
 	std::cout << "Loading glade\n";
 
@@ -113,7 +113,16 @@
 				imageColor.data[pixel+2] = 0;
 			}
 
+	// Save depth map in map vector (just save 25 maps)
+	std::vector<colorspaces::Image>::iterator it = mDepthVector.begin();
+	mDepthVector.insert(it, imageDepth);
 
+	if (mDepthVector.size() > MAX_MAPS)
+		mDepthVector.erase(mDepthVector.begin() + MAX_MAPS, mDepthVector.end());
+	else if (mDepthVector.size() == MAX_MAPS -1)
+		std::cout << "* Depth vector is already filled!" << std::endl;
+
+
 	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,
@@ -338,10 +347,8 @@
 	if (mCalibration){
 
 		pthread_mutex_lock(&mutex);
-		bool res = mCalibration->addPatternPixel (Eigen::Vector3d ((int) event->x,
-				(int) event->y,
-				1.0),
-				mImageDepth);
+		bool res = mCalibration->addPatternPixel (
+				Eigen::Vector3d ((int) event->x, (int) event->y, 1.0), mDepthVector);
 
 		if (res == false)
 		{

Modified: trunk/src/stable/components/rgbdCalibrator/viewer.h
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/viewer.h	2013-10-22 17:16:24 UTC (rev 1072)
+++ trunk/src/stable/components/rgbdCalibrator/viewer.h	2013-10-23 08:21:29 UTC (rev 1073)
@@ -95,6 +95,9 @@
     const HSV* hsvFilter;
     double hmin, hmax, smin, smax, vmin, vmax;
 
+    std::vector<colorspaces::Image> mDepthVector;
+    const int MAX_MAPS;
+
     // Extrinsics variables
 
     // onclicks



More information about the Jderobot-admin mailing list