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

rocapal en jderobot.org rocapal en jderobot.org
Mie Oct 23 12:06:42 CEST 2013


Author: rocapal
Date: 2013-10-23 12:06:42 +0200 (Wed, 23 Oct 2013)
New Revision: 1074

Modified:
   trunk/src/stable/components/rgbdCalibrator/calibration.cpp
   trunk/src/stable/components/rgbdCalibrator/calibration.h
   trunk/src/stable/components/rgbdCalibrator/viewer.cpp
   trunk/src/stable/components/rgbdCalibrator/viewer.h
Log:
#27 added some improvements about depth maps and distances


Modified: trunk/src/stable/components/rgbdCalibrator/calibration.cpp
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/calibration.cpp	2013-10-23 08:21:29 UTC (rev 1073)
+++ trunk/src/stable/components/rgbdCalibrator/calibration.cpp	2013-10-23 10:06:42 UTC (rev 1074)
@@ -242,13 +242,13 @@
 	RT(3,2) = double(0.);
 	RT(3,3) = double(1.);
 
-	/*
+
 	mProgeo = new Progeo::Progeo(posCamera, K, RT, 320, 240);
 	mProgeo->updateRTMatrix();
 	mProgeo->displayCameraInfo();
 	mProgeo->saveToFile("./CameraA.xml");
-	*/
 
+
 	/*
 	mProgeo = new Progeo::Progeo("./cameraB.xml");
 	mProgeo->updateKMatrix();
@@ -367,20 +367,23 @@
 	 */
 }
 
-void Calibration::getBestDepth (const Eigen::Vector3d pixel, const std::vector<colorspaces::Image> depthVector,
+void Calibration::getBestDepth (const Eigen::Vector3d pixel, 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)
+	res3D(2) = 1000000.0;
+
+
+	for (std::vector<colorspaces::Image>::iterator it = depthVector.begin() ; 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;
@@ -396,26 +399,23 @@
 				else
 					py =pixel(1)+y;
 
-				Eigen::Vector3d pixel(px,py,1.0);
+				Eigen::Vector3d pixelChoosen(px,py,1.0);
 				Eigen::Vector4d aux;
-				getRealPoint(pixel, (colorspaces::Image)(*it), aux);
 
-				if (aux(2) != 0 && aux(2) < res3D(2)) {
+				getRealPoint(pixelChoosen, (colorspaces::Image)*it, aux);
+
+				if (aux(2) != 0 && abs(aux(2)) < abs(res3D(2))) {
 					res3D = aux;
 					bestPixel = pixel;
 					depthData = *it;
 				}
 			}
 		}
-
 	}
-
-
-
 }
 
 
-bool Calibration::addPatternPixel (Eigen::Vector3d pixel, const std::vector<colorspaces::Image> depthVector)
+bool Calibration::addPatternPixel (Eigen::Vector3d pixel, std::vector<colorspaces::Image> depthVector)
 {
 	mPixelPoints.push_back(pixel);
 
@@ -429,8 +429,9 @@
 				  "Transformado Real\t\t" << "Pattern"
 				  <<std::endl;
 
-		const colorspaces::Image depthData;
 
+		colorspaces::Image depthData;
+
 		for (int i=0; i<mPixelPoints.size(); i++)
 		{
 
@@ -439,9 +440,11 @@
 					<< mPixelPoints[i](1) << ")  \t";
 
 			Eigen::Vector4d pCamera(0.,0.,1000000.,0.);
-
 			Eigen::Vector3d pixelGraphic;
 
+			getBestDepth(mPixelPoints[i], depthVector, depthData, pCamera, pixelGraphic);
+
+			/*
 			// 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++) {
@@ -470,6 +473,7 @@
 					}
 				}
 			}
+			*/
 
 			// We want to get the center of sphera (3cm or radius)
 			//pCamera(1) = pCamera(1) - 30;
@@ -571,10 +575,11 @@
 		Eigen::Vector4d& res3D)
 {
 
+	const float MIN_SIZE_XTION = 500.0;
+
 	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];
 
-
-	if (depth == 0)
+	if (depth == 0 || depth < MIN_SIZE_XTION )
 	{
 		res3D(0) = 0.;
 		res3D(1) = 0.;
@@ -644,12 +649,18 @@
 }
 
 
-void Calibration::test(Eigen::Vector3d pixel, const colorspaces::Image depthData)
+void Calibration::test(Eigen::Vector3d pixel, std::vector<colorspaces::Image> depthVector)
 {
 	std::cout << "====== TEST ====== " << std::endl;
 
-	Eigen::Vector4d pCamera;
-	getRealPoint(pixel, depthData, pCamera);
+	Eigen::Vector4d pCamera(0.,0.,1000000.,0.);
+	Eigen::Vector3d pixelGraphic;
+	colorspaces::Image depthData;
+
+	getBestDepth(pixel, depthVector, depthData, pCamera, pixelGraphic);
+
+	//getRealPoint(pixelGraphic, depthData, pCamera);
+
 	std::cout << "Pto ref Cámara: (" << pCamera(0) << "," << pCamera(1) << "," << pCamera(2) << ")  - ";
 	std::cout << "Dist: " << sqrt( (pCamera(0)*pCamera(0)) + (pCamera(1)*pCamera(1)) + (pCamera(2)*pCamera(2))) << std::endl;
 

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

Modified: trunk/src/stable/components/rgbdCalibrator/viewer.cpp
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/viewer.cpp	2013-10-23 08:21:29 UTC (rev 1073)
+++ trunk/src/stable/components/rgbdCalibrator/viewer.cpp	2013-10-23 10:06:42 UTC (rev 1074)
@@ -43,7 +43,7 @@
 
 Viewer::Viewer()
 : gtkmain(0,0),frameCount(0),
-  intrinsicsEnable(0),contPhoto(1),hsvFilter(NULL), mFrameBlob(NULL), MAX_MAPS(25) {
+  intrinsicsEnable(0),contPhoto(1),hsvFilter(NULL), mFrameBlob(NULL), MAX_MAPS(25), handlerDepth(true) {
 
 	std::cout << "Loading glade\n";
 
@@ -113,16 +113,22 @@
 				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);
+	// Save depth map in map vector (just save 25 maps). Each map is saved each 2 iterations
+	if (handlerDepth)
+	{
+		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;
+		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;
+	}
+	else
+		handlerDepth = !handlerDepth;
 
+
 	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,
@@ -353,7 +359,7 @@
 		if (res == false)
 		{
 			Eigen::Vector3d p2D ((int) event->x, (int) event->y, 1.0);
-			mCalibration->test(p2D, mImageDepth);
+			mCalibration->test(p2D, mDepthVector);
 		}
 
 		pthread_mutex_unlock(&mutex);

Modified: trunk/src/stable/components/rgbdCalibrator/viewer.h
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/viewer.h	2013-10-23 08:21:29 UTC (rev 1073)
+++ trunk/src/stable/components/rgbdCalibrator/viewer.h	2013-10-23 10:06:42 UTC (rev 1074)
@@ -97,6 +97,7 @@
 
     std::vector<colorspaces::Image> mDepthVector;
     const int MAX_MAPS;
+    bool handlerDepth;
 
     // Extrinsics variables
 



More information about the Jderobot-admin mailing list