[Jderobot-admin] jderobot-r976 - trunk/src/stable/components/rgbdCalibrator
rocapal en jderobot.org
rocapal en jderobot.org
Mie Ago 28 11:08:37 CEST 2013
Author: rocapal
Date: 2013-08-28 11:07:37 +0200 (Wed, 28 Aug 2013)
New Revision: 976
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/rgbdCalibrator.glade
trunk/src/stable/components/rgbdCalibrator/viewer.cpp
trunk/src/stable/components/rgbdCalibrator/viewer.h
Log:
[27] added depth and blob images
Modified: trunk/src/stable/components/rgbdCalibrator/calibration.cpp
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/calibration.cpp 2013-08-27 09:22:05 UTC (rev 975)
+++ trunk/src/stable/components/rgbdCalibrator/calibration.cpp 2013-08-28 09:07:37 UTC (rev 976)
@@ -21,7 +21,6 @@
#include <calibration.h>
#include <iostream>
-#include "geoUtils.h"
#include "../../libs/geometry/progeo/Progeo.h"
#include "../../libs/geometry/math/Point3D.h"
#include "../../libs/geometry/math/Segment3D.h"
@@ -39,6 +38,8 @@
Calibration::Calibration()
{}
+ Calibration::~Calibration()
+ {}
// Intrinsics
bool Calibration::runCalibrationAndSave(Size &boardSize,
@@ -60,7 +61,9 @@
std::cout << (ok ? "Calibration succeeded" : "Calibration failed")
<< ". avg re projection error = " << totalAvgErr ;
+ mKMatrix = Mat(cameraMatrix);
+
return ok;
}
@@ -461,4 +464,81 @@
delete(progeo);
}
+
+ void
+ Calibration::BackProjectWithDepth (const Eigen::Vector3d pixel,
+ const jderobot::ImageDataPtr depthData,
+ Eigen::Vector4d& res3D)
+ {
+
+
+ Eigen::Vector4d posCamera;
+ posCamera(0) = double(0.0);
+ posCamera(1) = double(0.0);
+ posCamera(2) = double(0.0);
+
+
+ Eigen::Matrix3d K;
+ K(0,0) = mKMatrix.at<double>(0,0);
+ K(0,1) = mKMatrix.at<double>(0,1);
+ K(0,2) = mKMatrix.at<double>(0,2);
+
+ K(1,0) = mKMatrix.at<double>(1,0);
+ K(1,1) = mKMatrix.at<double>(1,1);
+ K(1,2) = mKMatrix.at<double>(1,2);
+
+ K(2,0) = mKMatrix.at<double>(2,0);
+ K(2,1) = mKMatrix.at<double>(2,1);
+ K(2,2) = mKMatrix.at<double>(2,2);
+
+ Eigen::Matrix4d RT;
+ RT(0,0) = double(1.);
+ RT(0,1) = double(0.);
+ RT(0,2) = double(0.);
+ RT(0,3) = double(0.);
+
+ RT(1,0) = double(0.);
+ RT(1,1) = double(1.);
+ RT(1,2) = double(0.);
+ RT(1,3) = double(0.);
+
+ RT(2,0) = double(0.);
+ RT(2,1) = double(0.);
+ RT(2,2) = double(1.);
+ RT(2,3) = double(0.);
+
+ RT(3,0) = double(0.);
+ RT(3,1) = double(0.);
+ RT(3,2) = double(0.);
+ RT(3,3) = double(1.);
+
+
+ Progeo::Progeo* progeo = new Progeo::Progeo(posCamera, K, RT, 320, 240);
+ //progeo->display_camerainfo();
+
+
+ Eigen::Vector4d p3D;
+ progeo->backproject(pixel, p3D);
+
+ 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];
+
+ Point3D *nP3D = segment->getPointByZ(depth);
+
+ //Eigen::Vector4d* resP3D = new Eigen::Vector4d(nP3D->getPoint());
+
+ res3D = nP3D->getPoint();
+
+ delete(segment);
+ delete(pStart);
+ delete(pEnd);
+ delete(nP3D);
+ delete(progeo);
+
+ }
+
}
Modified: trunk/src/stable/components/rgbdCalibrator/calibration.h
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/calibration.h 2013-08-27 09:22:05 UTC (rev 975)
+++ trunk/src/stable/components/rgbdCalibrator/calibration.h 2013-08-28 09:07:37 UTC (rev 976)
@@ -30,6 +30,7 @@
#include <string>
#include <colorspaces/colorspacesmm.h>
#include <jderobot/camera.h>
+#include "../../libs/geometry/math/Point3D.h"
using namespace cv;
@@ -58,6 +59,7 @@
private:
+ Mat mKMatrix;
void calcBoardCornerPositions(Size boardSize,
float squareSize,
@@ -83,10 +85,17 @@
vector<Mat>& tvecs,
vector<float>& reprojErrs,
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-27 09:22:05 UTC (rev 975)
+++ trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.cpp 2013-08-28 09:07:37 UTC (rev 976)
@@ -70,7 +70,7 @@
&(data->pixelData[0]));
- /*
+
jderobot::ImageDataPtr data2 = cprxDepth->getImageData();
colorspaces::Image::FormatPtr fmt2 = colorspaces::Image::Format::searchFormat(data2->description->format);
if (!fmt2)
@@ -80,12 +80,12 @@
data2->description->height,
fmt2,
&(data2->pixelData[0]));
- */
+
//viewer.setDepth(NULL);//cprxDepth->getImageData());
- viewer.display(imgColor, imgColor);
+ viewer.display(imgColor, imgDepth);
Modified: trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.glade
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.glade 2013-08-27 09:22:05 UTC (rev 975)
+++ trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.glade 2013-08-28 09:07:37 UTC (rev 976)
@@ -257,6 +257,30 @@
<property name="y">25</property>
</packing>
</child>
+ <child>
+ <widget class="GtkImage" id="hsv_image">
+ <property name="width_request">320</property>
+ <property name="height_request">240</property>
+ <property name="visible">True</property>
+ <property name="stock">gtk-missing-image</property>
+ </widget>
+ <packing>
+ <property name="x">358</property>
+ <property name="y">306</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkImage" id="blob_image">
+ <property name="width_request">320</property>
+ <property name="height_request">240</property>
+ <property name="visible">True</property>
+ <property name="stock">gtk-missing-image</property>
+ </widget>
+ <packing>
+ <property name="x">688</property>
+ <property name="y">306</property>
+ </packing>
+ </child>
</widget>
</child>
</widget>
Modified: trunk/src/stable/components/rgbdCalibrator/viewer.cpp
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/viewer.cpp 2013-08-27 09:22:05 UTC (rev 975)
+++ trunk/src/stable/components/rgbdCalibrator/viewer.cpp 2013-08-28 09:07:37 UTC (rev 976)
@@ -24,7 +24,6 @@
#include <Ice/Ice.h>
#include <IceUtil/IceUtil.h>
#include <boost/filesystem.hpp>
-#include "calibration.h"
#include "../../libs/cvBlob/cvblob.h"
#define DEGTORAD (3.14159264 / 180.0)
@@ -53,6 +52,8 @@
refXml = Gnome::Glade::Xml::create(gladepath);
refXml->get_widget("color_image", gtkimage_color);
refXml->get_widget("depth_image", gtkimage_depth);
+ refXml->get_widget("hsv_image", gtkimage_hsv);
+ refXml->get_widget("blob_image", gtkimage_blob);
refXml->get_widget("mainwindow",mainwindow);
refXml->get_widget("fpslabel",fpsLabel);
refXml->get_widget("bt_take_photo", btTakePhoto);
@@ -74,6 +75,9 @@
RGB2HSV_init();
RGB2HSV_createTable();
+
+ mCalibration = new Calibration();
+
pthread_mutex_init(&mutex, NULL);
@@ -82,6 +86,7 @@
Viewer::~Viewer()
{
+ delete(mCalibration);
RGB2HSV_destroyTable();
}
@@ -118,10 +123,9 @@
if (hsvFilter != NULL)
{
createImageHSV();
-
-
- //colorspaces::ImageRGB8 img_rgb8D(imgHSV);
- Glib::RefPtr<Gdk::Pixbuf> imgBuffDepth =
+
+ // Show HSV image
+ Glib::RefPtr<Gdk::Pixbuf> imgBuffHSV =
Gdk::Pixbuf::create_from_data((const guint8*)imgHSV.data,
Gdk::COLORSPACE_RGB,
false,
@@ -130,8 +134,48 @@
imgHSV.size().height,
imgHSV.step);
+ gtkimage_hsv->clear();
+ gtkimage_hsv->set(imgBuffHSV);
+
+ // Show Blob Image
+
+ Glib::RefPtr<Gdk::Pixbuf> imgBuffBLOB =
+ Gdk::Pixbuf::create_from_data(
+ (guint8*)mFrameBlob->imageData,
+ Gdk::COLORSPACE_RGB,
+ false,
+ mFrameBlob->depth,
+ mFrameBlob->width,
+ mFrameBlob->height,
+ mFrameBlob->widthStep);
+
+ gtkimage_blob->clear();
+ gtkimage_blob->set(imgBuffBLOB);
+
+ cvReleaseImage(&mFrameBlob);
+
+ // Show depth image
+ colorspaces::ImageRGB8 img_rgb8D(imageDepth);
+ Glib::RefPtr<Gdk::Pixbuf> imgBuffDepth =
+ Gdk::Pixbuf::create_from_data((const guint8*)img_rgb8D.data,
+ Gdk::COLORSPACE_RGB,
+ false,
+ 8,
+ img_rgb8D.width,
+ img_rgb8D.height,
+ img_rgb8D.step);
+
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);
}
@@ -178,10 +222,10 @@
CvBlobs blobs;
IplImage *iplOrig = new IplImage(imgOrig);
- IplImage *frame=cvCreateImage(imgOrig.size(),8,3);
+ mFrameBlob=cvCreateImage(imgOrig.size(),8,3);
IplImage *labelImg=cvCreateImage(imgOrig.size(),IPL_DEPTH_LABEL,1);
- cvResize(iplOrig,frame,CV_INTER_LINEAR );
+ cvResize(iplOrig,mFrameBlob,CV_INTER_LINEAR );
//Threshy is a binary image
cvSmooth(threshy,threshy,CV_MEDIAN,7,7);
@@ -190,26 +234,39 @@
unsigned int result=cvLabel(threshy,labelImg,blobs);
//Rendering the blobs
- cvRenderBlobs(labelImg,blobs,frame,frame);
+ cvRenderBlobs(labelImg,blobs,mFrameBlob,mFrameBlob);
//Filter Blobs
- cvFilterByArea(blobs,300,1000);
+ cvFilterByArea(blobs,1000,2000);
+ double area = 0.0;
+ int x, y;
+
for (CvBlobs::const_iterator it=blobs.begin(); it!=blobs.end(); ++it)
{
- std::cout << "BLOB found: " << it->second->area <<std::endl;
+ //std::cout << "BLOB found: " << it->second->area <<std::endl;
+
double moment10 = it->second->m10;
double moment01 = it->second->m01;
- double area = it->second->area;
+
+ if (it->second->area >= area)
+ {
+ area = it->second->area;
+ x = moment10/area;
+ y = moment01/area;
+ }
}
- cvShowImage("Live",frame);
+ std::cout << "Max BLOB: " << area << ": " << x << " , " << y <<std::endl;
+ //cvShowImage("Live",mFrameBlob);
+
+
+
// Release and free memory
delete(iplOrig);
cvReleaseImage(&threshy);
- cvReleaseImage(&frame);
cvReleaseImage(&labelImg);
}
@@ -409,10 +466,10 @@
}
}
- Calibration* calib = new Calibration();
- calib->runCalibrationAndSave(boardSize, 20.0, flag, imageSize,
- cameraMatrix, distCoeffs, imagePoints);
+ mCalibration->runCalibrationAndSave(boardSize, 20.0, flag, imageSize,
+ cameraMatrix, distCoeffs, imagePoints);
+
std::cout << std::endl << cameraMatrix << std::endl;
std::stringstream matrixStr;
@@ -433,8 +490,9 @@
tvStatus->get_buffer()->set_text(matrixStr.str().c_str());
}
- calib->extrinsics(cameraMatrix, dataDepth);
+ mCalibration->extrinsics(cameraMatrix, dataDepth);
+
}
Modified: trunk/src/stable/components/rgbdCalibrator/viewer.h
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/viewer.h 2013-08-27 09:22:05 UTC (rev 975)
+++ trunk/src/stable/components/rgbdCalibrator/viewer.h 2013-08-28 09:07:37 UTC (rev 976)
@@ -32,6 +32,7 @@
#include <cmath>
#include <jderobot/camera.h>
#include <colorspaces/colorspaces.h>
+#include "calibration.h"
using namespace cv;
@@ -55,6 +56,8 @@
Glib::RefPtr<Gnome::Glade::Xml> refXml;
Gtk::Image* gtkimage_color;
Gtk::Image* gtkimage_depth;
+ Gtk::Image* gtkimage_hsv;
+ Gtk::Image* gtkimage_blob;
Gtk::Window* mainwindow;
Gtk::Label* fpsLabel;
Gtk::Label* lbSleepPhoto;
@@ -75,6 +78,7 @@
int frameCount;
// Intrinsics variables
+ Calibration* mCalibration;
int intrinsicsEnable;
IceUtil::Time lastTimePhoto;
int delayPhoto;
@@ -84,6 +88,7 @@
jderobot::ImageDataPtr dataDepth;
cv::Mat imgOrig;
cv::Mat imgHSV;
+ IplImage* mFrameBlob;
pthread_mutex_t mutex;
const HSV* hsvFilter;
double hmin, hmax, smin, smax, vmin, vmax;
More information about the Jderobot-admin
mailing list