[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