[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