[Jderobot-admin] jderobot-r1070 - trunk/src/stable/components/rgbdCalibrator
rocapal en jderobot.org
rocapal en jderobot.org
Mar Oct 22 17:57:32 CEST 2013
Author: rocapal
Date: 2013-10-22 17:57:32 +0200 (Tue, 22 Oct 2013)
New Revision: 1070
Modified:
trunk/src/stable/components/rgbdCalibrator/calibration.cpp
trunk/src/stable/components/rgbdCalibrator/calibration.h
trunk/src/stable/components/rgbdCalibrator/viewer.cpp
Log:
#27 added big pattern (tripod)
Modified: trunk/src/stable/components/rgbdCalibrator/calibration.cpp
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/calibration.cpp 2013-10-21 09:42:08 UTC (rev 1069)
+++ trunk/src/stable/components/rgbdCalibrator/calibration.cpp 2013-10-22 15:57:32 UTC (rev 1070)
@@ -21,16 +21,12 @@
#include <calibration.h>
#include <iostream>
-#include "../../libs/geometry/math/Point3D.h"
-#include "../../libs/geometry/math/Segment3D.h"
-#include <../../libs/progeo/progeo.h>
-#include <gsl/gsl_blas.h>
-#include <gsl/gsl_linalg.h>
-#include <gsl/gsl_matrix.h>
-#include <gsl/gsl_multifit.h>
+#include <progeo/progeo.h>
+
+
using namespace cv;
namespace rgbdCalibrator{
@@ -40,6 +36,7 @@
// You can see in OpenCV samples:
// - samples/cpp/tutorial_code/calib3d/camera_calibration/
+TPinHoleCamera myCamA;
Calibration::Calibration(): mProgeo(NULL)
{
@@ -178,7 +175,7 @@
void progeo_old_test ()
{
- TPinHoleCamera myCamA;
+
xmlReader(&myCamA, "./cameraB.xml");
display_camerainfo(myCamA);
@@ -186,13 +183,14 @@
display_camerainfo(myCamA);
HPoint2D pixel;
- pixel.x =224.;
- pixel.y =18.;
+ pixel.x =180.;
+ pixel.y =59.;
pixel.h = 1.;
std::cout << pixel.x << " , " << pixel.y << std::endl;
HPoint3D p3D;
+
backproject(&p3D, pixel, myCamA);
std::cout << p3D.X << " , " << p3D.Y << " , " << p3D.Z << std::endl;
@@ -244,18 +242,32 @@
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");
+ */
- Eigen::Vector3d pixel(224.0, 18., 1.0);
+ mProgeo = new Progeo::Progeo("./cameraB.xml");
+ mProgeo->updateKMatrix();
+ mProgeo->updateRTMatrix();
+ mProgeo->displayCameraInfo();
+
+ Eigen::Vector3d pixel(59.0, 59., 1.0);
Eigen::Vector4d p3d;
+
+ mProgeo->pixel2optical(pixel);
+
mProgeo->backproject(pixel,p3d);
std::cout << "Pixel: " << pixel << std::endl;
std::cout << "P3d: " << p3d << std::endl;
+ mProgeo->backprojectCV(pixel,p3d);
+ std::cout << "P3d: " << p3d << std::endl;
+
+
progeo_old_test();
}
@@ -304,26 +316,53 @@
void Calibration::initPatternPoints()
{
- // Down line, right to left
+
+ /* big pattern */
+/*
+ mPatternPoints.push_back(Eigen::Vector4d (-412.,0.,0.,1.));
+ mPatternPoints.push_back(Eigen::Vector4d (-262.,0.,0.,1.));
+ mPatternPoints.push_back(Eigen::Vector4d (-112.,0.,0.,1.));
+ mPatternPoints.push_back(Eigen::Vector4d (112.,0.,0.,1.));
+ mPatternPoints.push_back(Eigen::Vector4d (262.,0.,0.,1.));
+ mPatternPoints.push_back(Eigen::Vector4d (412.,0.,0.,1.));
+*/
+
+ mPatternPoints.push_back(Eigen::Vector4d (0.,-410.,630.,1.));
+ mPatternPoints.push_back(Eigen::Vector4d (0.,-260.,630.,1.));
+ mPatternPoints.push_back(Eigen::Vector4d (0.,-110.,630.,1.));
+ //mPatternPoints.push_back(Eigen::Vector4d (0.,110.,630.,1.));
+ //mPatternPoints.push_back(Eigen::Vector4d (0.,260.,630.,1.));
+ //mPatternPoints.push_back(Eigen::Vector4d (0.,410.,630.,1.));
+
+ mPatternPoints.push_back(Eigen::Vector4d (-408.,0.,1310.,1.));
+ mPatternPoints.push_back(Eigen::Vector4d (-258.,0.,1310.,1.));
+ mPatternPoints.push_back(Eigen::Vector4d (-108.,0.,1310.,1.));
+ mPatternPoints.push_back(Eigen::Vector4d (108.,0.,1310.,1.));
+ mPatternPoints.push_back(Eigen::Vector4d (258.,0.,1310.,1.));
+ mPatternPoints.push_back(Eigen::Vector4d (408.,0.,1310.,1.));
+
+ /* small pattern (cube)
+
+ // Down line, right to left
+ mPatternPoints.push_back(Eigen::Vector4d (0.,120.,0.,1.));
+ mPatternPoints.push_back(Eigen::Vector4d (0.,60.,0.,1.));
+ mPatternPoints.push_back(Eigen::Vector4d (0.,0.,0.,1.));
+ mPatternPoints.push_back(Eigen::Vector4d (60.,0.,0.,1.));
mPatternPoints.push_back(Eigen::Vector4d (120.,0.,0.,1.));
- mPatternPoints.push_back(Eigen::Vector4d (60.0,0.,0.,1.));
- mPatternPoints.push_back(Eigen::Vector4d (0.,0.,0.,1.));
- mPatternPoints.push_back(Eigen::Vector4d (0.,0.,60.,1.));
- mPatternPoints.push_back(Eigen::Vector4d (0.,0.,120.,1.));
// Up line, right to left
- mPatternPoints.push_back(Eigen::Vector4d (120.,120.,0.,1.));
- mPatternPoints.push_back(Eigen::Vector4d (60.,120.,0.,1.));
- mPatternPoints.push_back(Eigen::Vector4d (0.,120.,0.,1.));
- mPatternPoints.push_back(Eigen::Vector4d (0.,120.,60.,1.));
mPatternPoints.push_back(Eigen::Vector4d (0.,120.,120.,1.));
+ mPatternPoints.push_back(Eigen::Vector4d (0.,60.,120.,1.));
+ mPatternPoints.push_back(Eigen::Vector4d (0.,0.,120.,1.));
+ mPatternPoints.push_back(Eigen::Vector4d (60.,0.,120.,1.));
+ mPatternPoints.push_back(Eigen::Vector4d (120.,0.,120.,1.));
// Middle line, right to left
- mPatternPoints.push_back(Eigen::Vector4d (120.,60.,0.,1.));
- mPatternPoints.push_back(Eigen::Vector4d (0.,60.,0.,1.));
- mPatternPoints.push_back(Eigen::Vector4d (0.,60.,120.,1.));
+ mPatternPoints.push_back(Eigen::Vector4d (0.,120.,60.,1.));
+ mPatternPoints.push_back(Eigen::Vector4d (0.,0.,60.,1.));
+ mPatternPoints.push_back(Eigen::Vector4d (120.,0.,60.,1.));
-
+ */
}
bool Calibration::addPatternPixel (Eigen::Vector3d pixel, const colorspaces::Image depthData)
@@ -336,8 +375,8 @@
if (mPixelPoints.size() == mPatternPoints.size())
{
- std::cout << "\tPixels" << "\t\t" << "Pixel(Image)\tPixel(progeo)\tDistance" << "\t"
- << "Backproject" << "\t\t\t" << "Fran Point" << "\t\t" << "Pattern"
+ std::cout << "\tPixels" << "\t\t" << "Graphic Pixel\tOptical Pixel\tDistance" << "\t" <<
+ "Transformado Real\t\t" << "Pattern"
<<std::endl;
for (int i=0; i<mPixelPoints.size(); i++)
@@ -349,54 +388,98 @@
Eigen::Vector4d pCamera(0.,0.,1000000.,0.);
- Eigen::Vector3d pixelChoosen;
+ Eigen::Vector3d 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++) {
+ int px, py;
+ if (mPixelPoints[i](0)+x < 0)
+ px = 0;
+ else if (mPixelPoints[i](0)+x >= mProgeo->getImageWidth())
+ px = mProgeo->getImageWidth()-1;
+ else
+ px = mPixelPoints[i](0)+x;
- for (int y=-2;y<=2; y++) {
- Eigen::Vector3d pixel(mPixelPoints[i](0)+x,mPixelPoints[i](1)+y,1.0);
+ if (mPixelPoints[i](1)+y < 0)
+ py = 0;
+ else if (mPixelPoints[i](1)+y >= mProgeo->getImageHeight())
+ py = mProgeo->getImageHeight()-1;
+ else
+ py = mPixelPoints[i](1)+y;
+
+ Eigen::Vector3d pixel(px,py,1.0);
Eigen::Vector4d aux;
- BackProjectWithDepth (pixel, depthData, aux);
+ getRealPoint(pixel, depthData, aux);
if (aux(2) != 0 && aux(2) < pCamera(2)) {
pCamera = aux;
- pixelChoosen = pixel;
+ pixelGraphic = pixel;
}
}
}
+ // We want to get the center of sphera (3cm or radius)
+ //pCamera(1) = pCamera(1) - 30;
+ //pCamera(1) = pCamera(1) + 100;
+
mCameraPoints.push_back(pCamera);
- // Pixel Image
- std::cout << "(" << pixelChoosen[0] << "," << pixelChoosen[1] << ") \t";
+ // Pixel Graphic
+ std::cout << "(" << pixelGraphic[0] << "," << pixelGraphic[1] << ") \t";
- mProgeo->pixel2optical(pixelChoosen);
+ Eigen::Vector3d pixelOptical;
+ pixelOptical = pixelGraphic;
+ mProgeo->pixel2optical(pixelOptical);
// Pixel Progeo
- std::cout << "(" << pixelChoosen[0] << "," << pixelChoosen[1] << ") \t";
+ std::cout << "(" << pixelOptical[0] << "," << pixelOptical[1] << ") \t";
// Distance
- float depth = (int)depthData.data[((depthData.cols*(int)pixelChoosen(1))+(int)pixelChoosen(0))*3+1]<<8 | (int)depthData.data[((depthData.cols*(int)pixelChoosen(1))+(int)pixelChoosen(0))*3+2];
- std::cout << depth << "\t\t";
+ float depth = (int)depthData.data[((depthData.cols*(int)pixelGraphic(1))+(int)pixelGraphic(0))*3+1]<<8 | (int)depthData.data[((depthData.cols*(int)pixelGraphic(1))+(int)pixelGraphic(0))*3+2];
+ std::cout << depth << "\t";
+ /*
// Pixel backProject
Eigen::Vector4d aux3D;
- mProgeo->backproject(pixelChoosen, aux3D);
+ mProgeo->backproject(pixelOptical, aux3D);
std::cout << "(" << aux3D[0] << "," << aux3D[1] << "," << aux3D[2] << ")\t";
+ Eigen::Vector4d pos = mProgeo->getPosition();
+ Eigen::Vector4d p;
+ mProgeo->backproject(pixelOptical,p);
+
+ float modulo = sqrt(double(1/(((pos(0)-p(0))*(pos(0)-p(0)) + (pos(1)-p(1))*(pos(1)-p(1)) + (pos(2)-p(2))*(pos(2)-p(2))))));
+
+ Eigen::Vector4d u;
+ u(0) = (p(0)-pos(0)) * modulo;
+ u(1) = (p(1)-pos(1)) * modulo;
+ u(2) = (p(2)-pos(2)) * modulo;
+
+ float distance = (int)depthData.data[((depthData.cols*(int)pixelGraphic(1))+(int)pixelGraphic(0))*3+1]<<8 | (int)depthData.data[((depthData.cols*(int)pixelGraphic(1))+(int)pixelGraphic(0))*3+2];
+
+ Eigen::Vector4d p3DPH;
+ p3DPH(0) = distance*u(0) + pos(0);
+ p3DPH(1) = distance*u(1) + pos(0);
+ p3DPH(2) = distance*u(2) + pos(0);
+
+ std::cout << "\t(" << p3DPH[0] << "," << p3DPH[1] << "," << p3DPH[2] << ")\t";
+ */
+
// fix bug about projection's progeo
Eigen::Vector4d p3D;
- getRealPoint(pixelChoosen, depthData, p3D);
+ getRealPoint(pixelGraphic, depthData, p3D);
std::cout << "\t(" << p3D[0] << "," << p3D[1] << "," << p3D[2] << ")\t\t";
-
std::cout << "(" << mPatternPoints[i](0) << ","
<< mPatternPoints[i](1) << ","
<< mPatternPoints[i](2) << ")";
std::cout << std::endl;
+
+
+
}
mPairPoints.clear();
@@ -438,6 +521,7 @@
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)
{
res3D(0) = 0.;
@@ -448,7 +532,9 @@
}
Eigen::Vector4d p3D;
- mProgeo->backproject(pixel, p3D);
+ Eigen::Vector3d pixelOptical = pixel;
+ mProgeo->pixel2optical(pixelOptical);
+ mProgeo->backproject(pixelOptical, p3D);
//std::cout << "-> p3D: " << p3D << std::endl;
@@ -502,22 +588,32 @@
res3D(0) = t*u(0) + camPosition(0);
res3D(1) = t*u(1) + camPosition(1);
res3D(2) = t*u(2) + camPosition(2);
-
+ res3D(3) = 1.0;
}
-void Calibration::test(Eigen::Vector4d pCamera)
+void Calibration::test(Eigen::Vector3d pixel, const colorspaces::Image depthData)
{
std::cout << "====== TEST ====== " << std::endl;
- std::cout << pCamera << std::endl;
+ Eigen::Vector4d pCamera;
+ getRealPoint(pixel, 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;
+
Eigen::Vector4d s = mRTsolution * pCamera;
- std::cout << s << std::endl;
+ std::cout << "Pto ref Patron: (" << s(0) << "," << s(1) << "," << s(2) << ") - ";
+
+ std::cout << "Dist: " << sqrt( (s(0)*s(0)) + (s(1)*s(1)) + (s(2)*s(2))) << std::endl;
+
+ 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 << "Real Dist Xtion: " << depth << std::endl;
}
void Calibration::LSO()
{
+
gsl_matrix *ertm=gsl_matrix_alloc(4,4);
double chisq;
gsl_matrix *x,*cov;
@@ -530,8 +626,8 @@
c=gsl_vector_alloc(12);
work = gsl_multifit_linear_alloc (mPairPoints.size()*3,12);
double x1,y1,z1,x2,y2,z2;
- //preaparamos la matriz de ecuaciones
+
for (int i=0; i<mPairPoints.size(); i++){
x1=mPairPoints[i].first(0);
y1=mPairPoints[i].first(1);
@@ -559,6 +655,8 @@
gsl_vector_set(y,i*3+2, z1);
}
+ //print_gsl_matrix(x);
+
gsl_multifit_linear(x,y,c,cov,&chisq,work);
gsl_multifit_linear_free(work);
@@ -587,9 +685,23 @@
//gsl_matrix_fprintf(stdout,ertm,"%f");
std::cout << mRTsolution << std::endl;
+}
+void Calibration::print_gsl_matrix (const gsl_matrix *m)
+{
+ for (size_t i = 0; i < m->size1; i++) {
+ for (size_t j = 0; j < m->size2; j++) {
+
+ double item = gsl_matrix_get(m, i, j);
+ std::cout << item << "\t";
+ }
+
+ std::cout << std::endl;
+ }
+ std::cout << std::endl << std::endl;
}
+
}
Modified: trunk/src/stable/components/rgbdCalibrator/calibration.h
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/calibration.h 2013-10-21 09:42:08 UTC (rev 1069)
+++ trunk/src/stable/components/rgbdCalibrator/calibration.h 2013-10-22 15:57:32 UTC (rev 1070)
@@ -28,12 +28,24 @@
#include <opencv2/highgui/highgui.hpp>
#include <cmath>
#include <string>
+
+#include <boost/tuple/tuple.hpp>
+#include <boost/random/linear_congruential.hpp>
+#include <boost/random/uniform_int.hpp>
+#include <boost/random/uniform_real.hpp>
+#include <boost/random/variate_generator.hpp>
+
+#include <geometry/math/Point3D.h>
+#include <geometry/math/Segment3D.h>
+#include <geometry/progeo/Progeo.h>
#include <visionlib/colorspaces/colorspacesmm.h>
#include <jderobot/camera.h>
-#include "../../libs/geometry/math/Point3D.h"
-#include <boost/tuple/tuple.hpp>
-#include "../../libs/geometry/progeo/Progeo.h"
+#include <gsl/gsl_blas.h>
+#include <gsl/gsl_linalg.h>
+#include <gsl/gsl_matrix.h>
+#include <gsl/gsl_multifit.h>
+
using namespace cv;
namespace rgbdCalibrator
@@ -66,7 +78,7 @@
Eigen::Matrix4d getRTSolution() { return mRTsolution; };
- void test(Eigen::Vector4d pCamera);
+ void test(Eigen::Vector3d pixel, const colorspaces::Image depthData);
private:
@@ -90,7 +102,7 @@
void initPatternPoints();
void LSO();
-
+ void print_gsl_matrix (const gsl_matrix *m);
void calcBoardCornerPositions(Size boardSize,
float squareSize,
Modified: trunk/src/stable/components/rgbdCalibrator/viewer.cpp
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/viewer.cpp 2013-10-21 09:42:08 UTC (rev 1069)
+++ trunk/src/stable/components/rgbdCalibrator/viewer.cpp 2013-10-22 15:57:32 UTC (rev 1070)
@@ -38,7 +38,7 @@
const std::string gladepath = std::string(GLADE_DIR) +
std::string("/rgbdCalibrator.glade");
-const std::string pathImage = "./images2/";
+const std::string pathImage = "./images/";
Viewer::Viewer()
@@ -102,6 +102,18 @@
void Viewer::display( const colorspaces::Image& imageColor, const colorspaces::Image& imageDepth )
{
+
+ // Draw optical center
+ for (int i = -2; i<=2; i++)
+ for (int j =-2; j<=2; j++)
+ {
+ int pixel = (imageColor.width*(imageColor.height/2+j)+(imageColor.width/2+i))*3;
+ imageColor.data[pixel] = 255;
+ imageColor.data[pixel+1] = 0;
+ imageColor.data[pixel+2] = 0;
+ }
+
+
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,
@@ -115,6 +127,8 @@
gtkimage_color->clear();
gtkimage_color->set(imgBuffColor);
+
+
gtkimage_color2->clear();
gtkimage_color2->set(imgBuffColor);
@@ -332,9 +346,7 @@
if (res == false)
{
Eigen::Vector3d p2D ((int) event->x, (int) event->y, 1.0);
- Eigen::Vector4d p3D;
- mCalibration->BackProjectWithDepth (p2D, mImageDepth, p3D);
- mCalibration->test(p3D);
+ mCalibration->test(p2D, mImageDepth);
}
pthread_mutex_unlock(&mutex);
More information about the Jderobot-admin
mailing list