[Jderobot-admin] jderobot-r970 - in trunk/src/stable/components: . rgbdCalibrator
rocapal en jderobot.org
rocapal en jderobot.org
Lun Ago 12 12:54:01 CEST 2013
Author: rocapal
Date: 2013-08-12 12:53:01 +0200 (Mon, 12 Aug 2013)
New Revision: 970
Added:
trunk/src/stable/components/rgbdCalibrator/
trunk/src/stable/components/rgbdCalibrator/CMakeLists.txt
trunk/src/stable/components/rgbdCalibrator/beep.wav
trunk/src/stable/components/rgbdCalibrator/calibration.cpp
trunk/src/stable/components/rgbdCalibrator/calibration.h
trunk/src/stable/components/rgbdCalibrator/geoUtils.cpp
trunk/src/stable/components/rgbdCalibrator/geoUtils.h
trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.cfg
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 intrinsics params obtained with opencv
Added: trunk/src/stable/components/rgbdCalibrator/CMakeLists.txt
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/CMakeLists.txt (rev 0)
+++ trunk/src/stable/components/rgbdCalibrator/CMakeLists.txt 2013-08-12 10:53:01 UTC (rev 970)
@@ -0,0 +1,25 @@
+SET( SOURCE_FILES rgbdCalibrator.cpp viewer.cpp calibration.cpp geoUtils.cpp)
+
+add_definitions(-DGLADE_DIR="${gladedir}")
+
+include_directories(
+ ${INTERFACES_CPP_DIR}
+ ${LIBS_DIR}
+ ${CMAKE_CURRENT_SOURCE_DIR}
+)
+
+add_executable (rgbdCalibrator ${SOURCE_FILES})
+
+TARGET_LINK_LIBRARIES(rgbdCalibrator
+ ${CMAKE_THREAD_LIBS_INIT}
+ ${alut_LIBRARIES}
+ ${OpenCV_LIBRARIES}
+ ${gtkmm_LIBRARIES}
+ ${libglademm_LIBRARIES}
+ ${LIBS_DIR}/colorspaces/libcolorspacesmm.so
+ ${INTERFACES_CPP_DIR}/jderobot/libJderobotInterfaces.so
+ ${ZeroCIce_LIBRARIES}
+ ${Boost_LIBRARIES}
+ ${LIBS_DIR}/progeo/libprogeo.so
+ ${LIBS_DIR}/geometry/libgeometry.so
+)
Added: trunk/src/stable/components/rgbdCalibrator/beep.wav
===================================================================
(Binary files differ)
Property changes on: trunk/src/stable/components/rgbdCalibrator/beep.wav
___________________________________________________________________
Added: svn:mime-type
+ application/octet-stream
Added: trunk/src/stable/components/rgbdCalibrator/calibration.cpp
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/calibration.cpp (rev 0)
+++ trunk/src/stable/components/rgbdCalibrator/calibration.cpp 2013-08-12 10:53:01 UTC (rev 970)
@@ -0,0 +1,464 @@
+/*
+ *
+ * Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ *
+ * Authors : Roberto Calvo <rocapal [at] gsyc [dot] urjc [dot] es>
+ *
+ */
+
+#include <calibration.h>
+#include <iostream>
+#include "geoUtils.h"
+#include <jderobot/progeo/Progeo.h>
+#include <jderobot/math/Point3D.h>
+#include <jderobot/math/Segment3D.h>
+
+using namespace cv;
+
+namespace rgbdCalibrator{
+
+
+ // This source code is a modification of camera_calibration.cpp
+ // You can see in OpenCV samples:
+ // - samples/cpp/tutorial_code/calib3d/camera_calibration/
+
+
+ Calibration::Calibration()
+ {}
+
+
+ // Intrinsics
+ bool Calibration::runCalibrationAndSave(Size &boardSize,
+ float squareSize,
+ int flag,
+ Size imageSize,
+ Mat& cameraMatrix,
+ Mat& distCoeffs,
+ vector<vector<Point2f> > imagePoints )
+ {
+ vector<Mat> rvecs, tvecs;
+ vector<float> reprojErrs;
+ double totalAvgErr = 0;
+
+ bool ok = runCalibration (boardSize, squareSize, flag, imageSize,
+ cameraMatrix, distCoeffs, imagePoints,
+ rvecs, tvecs, reprojErrs, totalAvgErr);
+
+ std::cout << (ok ? "Calibration succeeded" : "Calibration failed")
+ << ". avg re projection error = " << totalAvgErr ;
+
+
+ return ok;
+ }
+
+ bool Calibration::runCalibration(Size &boardSize,
+ float squareSize,
+ int flag,
+ Size& imageSize,
+ Mat& cameraMatrix,
+ Mat& distCoeffs,
+ vector<vector<Point2f> > imagePoints,
+ vector<Mat>& rvecs,
+ vector<Mat>& tvecs,
+ vector<float>& reprojErrs,
+ double& totalAvgErr)
+ {
+
+ Pattern calibrationPattern = CHESSBOARD;
+
+ cameraMatrix = Mat::eye(3, 3, CV_64F);
+ if( flag & CV_CALIB_FIX_ASPECT_RATIO )
+ cameraMatrix.at<double>(0,0) = 1.0;
+
+ distCoeffs = Mat::zeros(8, 1, CV_64F);
+
+ vector<vector<Point3f> > objectPoints(1);
+ calcBoardCornerPositions(boardSize, squareSize, objectPoints[0], calibrationPattern);
+
+ objectPoints.resize(imagePoints.size(),objectPoints[0]);
+
+ //Find intrinsic and extrinsic camera parameters
+ double rms = calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix,
+ distCoeffs, rvecs, tvecs, flag|CV_CALIB_FIX_K4|CV_CALIB_FIX_K5);
+
+ std::cout << "Re-projection error reported by calibrateCamera: "<< rms << std::endl;
+
+ bool ok = checkRange(cameraMatrix) && checkRange(distCoeffs);
+
+ totalAvgErr = computeReprojectionErrors(objectPoints, imagePoints,
+ rvecs, tvecs, cameraMatrix, distCoeffs, reprojErrs);
+
+ return ok;
+}
+
+
+ void Calibration::calcBoardCornerPositions(Size boardSize,
+ float squareSize,
+ vector<Point3f>& corners,
+ Pattern patternType /*= Settings::CHESSBOARD*/)
+ {
+ corners.clear();
+
+ switch(patternType)
+ {
+ case CHESSBOARD:
+ case CIRCLES_GRID:
+ for( int i = 0; i < boardSize.height; ++i )
+ for( int j = 0; j < boardSize.width; ++j )
+ corners.push_back(Point3f(float( j*squareSize ), float( i*squareSize ), 0));
+ break;
+
+ case ASYMMETRIC_CIRCLES_GRID:
+ for( int i = 0; i < boardSize.height; i++ )
+ for( int j = 0; j < boardSize.width; j++ )
+ corners.push_back(Point3f(float((2*j + i % 2)*squareSize), float(i*squareSize), 0));
+ break;
+ default:
+ break;
+ }
+ }
+
+
+
+ double Calibration::computeReprojectionErrors( const vector<vector<Point3f> >& objectPoints,
+ const vector<vector<Point2f> >& imagePoints,
+ const vector<Mat>& rvecs,
+ const vector<Mat>& tvecs,
+ const Mat& cameraMatrix,
+ const Mat& distCoeffs,
+ vector<float>& perViewErrors)
+ {
+ vector<Point2f> imagePoints2;
+ int i, totalPoints = 0;
+ double totalErr = 0, err;
+ perViewErrors.resize(objectPoints.size());
+
+ for( i = 0; i < (int)objectPoints.size(); ++i )
+ {
+ projectPoints( Mat(objectPoints[i]), rvecs[i], tvecs[i], cameraMatrix,
+ distCoeffs, imagePoints2);
+ err = norm(Mat(imagePoints[i]), Mat(imagePoints2), CV_L2);
+
+ int n = (int)objectPoints[i].size();
+ perViewErrors[i] = (float) std::sqrt(err*err/n);
+ totalErr += err*err;
+ totalPoints += n;
+ }
+
+ return std::sqrt(totalErr/totalPoints);
+ }
+
+
+ // Extrinsics
+ void Calibration::extrinsics (const Mat& kMatrix,
+ const jderobot::ImageDataPtr depthData)
+
+ {
+
+ /*
+
+ TPinHoleCamera m_progeoCam;
+
+ // Camera Position
+ HPoint3D positionCam;
+
+ positionCam.X= 0;
+ positionCam.Y= 0;
+ positionCam.Z= 0;
+ positionCam.H= 0.0;
+
+ m_progeoCam.position = positionCam;
+
+ // K Matrix
+ m_progeoCam.k11 = kMatrix.at<double>(0,0);
+ m_progeoCam.k12 = kMatrix.at<double>(0,1);
+ m_progeoCam.k13 = kMatrix.at<double>(0,2);
+ m_progeoCam.k14 = 0;
+
+ m_progeoCam.k21 = kMatrix.at<double>(1,0);
+ m_progeoCam.k22 = kMatrix.at<double>(1,1);
+ m_progeoCam.k23 = kMatrix.at<double>(1,2);
+ m_progeoCam.k24 = 0;
+
+ m_progeoCam.k31 = kMatrix.at<double>(2,0);
+ m_progeoCam.k32 = kMatrix.at<double>(2,1);
+ m_progeoCam.k33 = kMatrix.at<double>(2,2);
+ m_progeoCam.k34 = 0;
+
+ // RT Matrix
+ m_progeoCam.rt11 = 1;
+ m_progeoCam.rt12 = 0;
+ m_progeoCam.rt13 = 0;
+ m_progeoCam.rt14 = 0;
+
+ m_progeoCam.rt21 = 0;
+ m_progeoCam.rt22 = 1;
+ m_progeoCam.rt23 = 0;
+ m_progeoCam.rt24 = 0;
+
+ m_progeoCam.rt31 = 0;
+ m_progeoCam.rt32 = 0;
+ m_progeoCam.rt33 = 1;
+ m_progeoCam.rt34 = 0;
+
+ m_progeoCam.rt41 = 0;
+ m_progeoCam.rt42 = 0;
+ m_progeoCam.rt43 = 0;
+ m_progeoCam.rt44 = 1;
+
+
+ display_camerainfo(m_progeoCam);
+ // backproject
+ HPoint3D point3D;
+ HPoint2D point2D;
+
+ point2D.x = 180,0;
+ point2D.y = 180.0;
+ point2D.h = 1.0;
+
+ printf ("Pto en 2D: %2.2f %2.2f %2.2f \n",point2D.x,point2D.y,point2D.h);
+
+ if (backproject(&point3D,point2D,m_progeoCam)!=-1)
+ printf("Backproject a 3D (de nuevo): %.2f %.2f %.2f %.2f \n",point3D.X, point3D.Y, point3D.Z, point3D.H);
+
+
+ float depth = (int)depthData->pixelData[((depthData->description->width*(int)point2D.y)+(int)point2D.x)*3+1]<<8 |
+ (int)depthData->pixelData[((depthData->description->width*(int)point2D.y)+(int)point2D.x)*3+2];
+
+
+ std::cout << depth << std::endl;
+ std::cout << depthData->description->width << std::endl;
+
+ HPoint2D p2;
+ if (project(point3D, &p2, m_progeoCam))
+ printf("Project a 2D (de nuevo): %2.2f,%2.2f,%2.2f \n",p2.x,p2.y,p2.h);
+
+ point3D.X = 150.64;
+ point3D.Y = 444.57;
+ point3D.Z = 1000.0;
+ point3D.H = 1.0;
+
+ if (project(point3D, &p2, m_progeoCam))
+ printf("Project a 2D (): %2.2f,%2.2f,%2.2f \n",p2.x,p2.y,p2.h);
+
+ std::cout << "_----------------------_" << std::endl;
+ point2D.x = 180,0;
+ point2D.y = 180.0;
+ point2D.h = 1.0;
+
+ printf ("Pixek 2D: %2.2f %2.2f %2.2f \n",point2D.x,point2D.y,point2D.h);
+
+ if (backproject(&point3D,point2D,m_progeoCam)!=-1)
+ printf("Backproject a 3D (de nuevo): %.2f %.2f %.2f %.2f \n",point3D.X, point3D.Y, point3D.Z, point3D.H);
+
+ HPoint3D orig;
+ orig.X = 0.;
+ orig.Y = 0.;
+ orig.Z = 0.;
+ orig.H = 1.;
+
+ GeoUtils* gUtils = new GeoUtils();
+ HPoint3D vector = gUtils->getVector(orig, point3D);
+
+ GeoUtils::Line3D* line3D = new GeoUtils::Line3D(orig, vector);
+
+ HPoint3D p3 = line3D->getPointByZ(2000.0);
+
+ printf("getPointByZ : %.2f %.2f %.2f %.2f \n",p3.X, p3.Y, p3.Z, p3.H);
+
+ if (project(p3, &p2, m_progeoCam))
+ printf("Project a 2D (): %2.2f,%2.2f,%2.2f \n",p2.x,p2.y,p2.h);
+
+ delete(line3D);
+ delete (gUtils);
+
+
+ std::cout << "_----------------------_" << std::endl;
+
+ */
+
+ /*
+
+ math::Vector3H pos;
+ pos.vector(0) = 0.0;
+ pos.vector(1) = 0.0;
+ pos.vector(2) = 0.0;
+ pos.vector(3) = 0.0;
+
+ math::Matriz3x3 K;
+ K.getMatriz()(0,0) = kMatrix.at<double>(0,0);
+ K.getMatriz()(0,1) = kMatrix.at<double>(0,1);
+ K.getMatriz()(0,2) = kMatrix.at<double>(0,2);
+
+ K.getMatriz()(1,0) = kMatrix.at<double>(1,0);
+ K.getMatriz()(1,1) = kMatrix.at<double>(1,1);
+ K.getMatriz()(1,2) = kMatrix.at<double>(1,2);
+
+ K.getMatriz()(2,0) = kMatrix.at<double>(2,0);
+ K.getMatriz()(2,1) = kMatrix.at<double>(2,1);
+ K.getMatriz()(2,2) = kMatrix.at<double>(2,2);
+
+ math::Matriz4x4 RT;
+ RT.getMatrix()(0,0) = 1.;
+ RT.getMatrix()(0,1) = 0.;
+ RT.getMatrix()(0,2) = 0.;
+ RT.getMatrix()(0,3) = 0.;
+
+ RT.getMatrix()(1,0) = 0.;
+ RT.getMatrix()(1,1) = 1.;
+ RT.getMatrix()(1,2) = 0.;
+ RT.getMatrix()(1,3) = 0.;
+
+ RT.getMatrix()(2,0) = 0.;
+ RT.getMatrix()(2,1) = 0.;
+ RT.getMatrix()(2,2) = 1.;
+ RT.getMatrix()(2,3) = 0.;
+
+ RT.getMatrix()(3,0) = 0.;
+ RT.getMatrix()(3,1) = 0.;
+ RT.getMatrix()(3,2) = 0.;
+ RT.getMatrix()(3,3) = 1.;
+
+
+ Progeo::Progeo* progeo = new Progeo::Progeo(pos, K, RT, 320, 240);
+ progeo->display_camerainfo();
+
+ math::Vector3H p3D;
+ math::Vector2H pixel (10.0, 100.0, 1.0);
+
+ progeo->backproject(pixel, p3D);
+
+ std::cout << p3D << std::endl;
+
+ HPoint3D orig;
+ orig.X = 0.;
+ orig.Y = 0.;
+ orig.Z = 0.;
+ orig.H = 1.;
+
+ HPoint3D point3D;
+ point3D.X = p3D.getX();
+ point3D.Y = p3D.getY();
+ point3D.Z = p3D.getZ();
+ point3D.H = 1.;
+
+ GeoUtils* gUtils = new GeoUtils();
+ HPoint3D vector = gUtils->getVector(orig, point3D);
+
+ GeoUtils::Line3D* line3D = new GeoUtils::Line3D(orig, vector);
+
+ HPoint3D p3 = line3D->getPointByZ(2000.0);
+
+ printf("getPointByZ : %.2f %.2f %.2f %.2f \n",p3.X, p3.Y, p3.Z, p3.H);
+
+
+ p3D.setX(p3.X);
+ p3D.setY(p3.Y);
+ p3D.setZ(p3.Z);
+ p3D.setH(1.0);
+
+ std::cout << p3D << std::endl;
+
+ math::Vector2H pixel2;
+ progeo->project(p3D, pixel2);
+
+ std::cout << pixel2.getX() << " " << pixel2.getY() << std::endl;
+
+ delete(line3D);
+ delete (gUtils);
+ delete(progeo);
+
+ */
+
+ Eigen::Vector4d posCamera;
+ posCamera(0) = double(0.0);
+ posCamera(1) = double(0.0);
+ posCamera(2) = double(0.0);
+
+
+ Eigen::Matrix3d K;
+ K(0,0) = kMatrix.at<double>(0,0);
+ K(0,1) = kMatrix.at<double>(0,1);
+ K(0,2) = kMatrix.at<double>(0,2);
+
+ K(1,0) = kMatrix.at<double>(1,0);
+ K(1,1) = kMatrix.at<double>(1,1);
+ K(1,2) = kMatrix.at<double>(1,2);
+
+ K(2,0) = kMatrix.at<double>(2,0);
+ K(2,1) = kMatrix.at<double>(2,1);
+ K(2,2) = kMatrix.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;
+ Eigen::Vector3d pixel;
+
+ pixel(0) = double(180.);
+ pixel(1) = double(180.);
+ pixel(2) = double(1.);
+
+ progeo->backproject(pixel, p3D);
+
+ std::cout << "Punto 3D: " << p3D << std::endl;
+
+ progeo->project(p3D, pixel);
+
+ std::cout << "Pixel 2D: " << pixel << std::endl;
+
+ Point3D *pStart = new Point3D(0.0,0.0,0.0);
+ Point3D *pEnd = new Point3D(p3D);
+
+ Segment3D *segment = new Segment3D(*pStart,*pEnd);
+ Point3D *nP3D = segment->getPointByZ(200.0);
+
+ std::cout << "New Point 3D with Z=200.0 " << *nP3D << std::endl;
+
+ progeo->project(nP3D->getPoint(), pixel);
+
+ std::cout << "Project Pixel\n" << pixel << std::endl;
+
+ delete(pStart);
+ delete(pEnd);
+ delete(nP3D);
+ delete(segment);
+ delete(progeo);
+ }
+
+}
Added: trunk/src/stable/components/rgbdCalibrator/calibration.h
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/calibration.h (rev 0)
+++ trunk/src/stable/components/rgbdCalibrator/calibration.h 2013-08-12 10:53:01 UTC (rev 970)
@@ -0,0 +1,93 @@
+/*
+ *
+ * Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ *
+ * Authors : Roberto Calvo <rocapal [at] gsyc [dot] urjc [dot] es>
+ *
+ */
+
+#ifndef RGBDCALIBRATOR_CALIBRATION_H
+#define RGBDCALIBRATOR_CALIBRATION_H
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/calib3d/calib3d.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <cmath>
+#include <string>
+#include <colorspaces/colorspacesmm.h>
+#include <jderobot/camera.h>
+
+using namespace cv;
+
+namespace rgbdCalibrator
+{
+
+ class Calibration
+ {
+
+ public:
+
+ Calibration();
+ ~Calibration();
+
+ enum Pattern { NOT_EXISTING, CHESSBOARD, CIRCLES_GRID, ASYMMETRIC_CIRCLES_GRID };
+
+ bool runCalibrationAndSave(Size &boardSize,
+ float squareSize,
+ int flag,
+ Size imageSize,
+ Mat& cameraMatrix,
+ Mat& distCoeffs,
+ vector<vector<Point2f> > imagePoints );
+
+ void extrinsics (const Mat& kMatrix, const jderobot::ImageDataPtr depth);
+
+ private:
+
+
+ void calcBoardCornerPositions(Size boardSize,
+ float squareSize,
+ vector<Point3f>& corners,
+ Pattern patternType /*= Settings::CHESSBOARD*/);
+
+ double computeReprojectionErrors( const vector<vector<Point3f> >& objectPoints,
+ const vector<vector<Point2f> >& imagePoints,
+ const vector<Mat>& rvecs,
+ const vector<Mat>& tvecs,
+ const Mat& cameraMatrix ,
+ const Mat& distCoeffs,
+ vector<float>& perViewErrors);
+
+ bool runCalibration(Size &boardSize,
+ float squareSize,
+ int flag,
+ Size& imageSize,
+ Mat& cameraMatrix,
+ Mat& distCoeffs,
+ vector<vector<Point2f> > imagePoints,
+ vector<Mat>& rvecs,
+ vector<Mat>& tvecs,
+ vector<float>& reprojErrs,
+ double& totalAvgErr);
+
+ };
+
+
+}
+
+
+#endif //RGBDCALIBRATOR_CALIBRATION_H
Added: trunk/src/stable/components/rgbdCalibrator/geoUtils.cpp
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/geoUtils.cpp (rev 0)
+++ trunk/src/stable/components/rgbdCalibrator/geoUtils.cpp 2013-08-12 10:53:01 UTC (rev 970)
@@ -0,0 +1,75 @@
+/*
+ *
+ * Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ *
+ * Authors : Roberto Calvo <rocapal [at] gsyc [dot] urjc [dot] es>
+ *
+ */
+
+#include "geoUtils.h"
+
+namespace rgbdCalibrator
+{
+
+ GeoUtils::Line3D::Line3D ()
+ {
+ }
+
+ GeoUtils::Line3D::Line3D (HPoint3D p, HPoint3D v)
+ {
+ point = p;
+ vector = v;
+ }
+
+ bool GeoUtils::Line3D::isPointInLine (HPoint3D p)
+ {
+ return ((p.X - point.X)/vector.X) ==
+ ((p.Y - point.Y)/vector.Y) ==
+ ((p.Z - point.Z)/vector.Z);
+ }
+
+ HPoint3D GeoUtils::Line3D::getPointByZ (float z)
+ {
+ HPoint3D res;
+ t = (z - point.Z)/ vector.Z;
+
+ res.X = point.X + t*vector.X;
+ res.Y = point.Y + t*vector.Y;
+ res.Z = z;
+ res.H = 1.0;
+
+ return res;
+ }
+
+
+ GeoUtils::GeoUtils ()
+ {
+ }
+
+ HPoint3D GeoUtils::getVector (HPoint3D p0, HPoint3D p1)
+ {
+ HPoint3D vector;
+ vector.X = p0.X - p1.X;
+ vector.Y = p0.Y - p1.Y;
+ vector.Z = p0.Z - p1.Z;
+ vector.H = 1.;
+
+ return vector;
+ }
+
+
+
+}
Added: trunk/src/stable/components/rgbdCalibrator/geoUtils.h
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/geoUtils.h (rev 0)
+++ trunk/src/stable/components/rgbdCalibrator/geoUtils.h 2013-08-12 10:53:01 UTC (rev 970)
@@ -0,0 +1,76 @@
+/*
+ *
+ * Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ *
+ * Authors : Roberto Calvo <rocapal [at] gsyc [dot] urjc [dot] es>
+ *
+ */
+
+#include <iostream>
+#include "viewer.h"
+#include <progeo/progeo.h>
+
+namespace rgbdCalibrator
+{
+
+ class GeoUtils
+ {
+
+ public:
+
+ class Line3D
+ {
+ public:
+
+ Line3D();
+
+ Line3D(HPoint3D p, HPoint3D v);
+
+ bool isPointInLine (HPoint3D p);
+
+ HPoint3D getPointByZ (float z);
+
+
+ private:
+ // parametric equations
+ // (x,y,z) = (x0,y0,z0) + t(x1,y1,z1)
+ //
+ // (x0,y0,z0) is a point
+ // (x1,y1,z1) is a director vector
+
+ // catesian equations
+ // x = x0 + t*x1
+ // y = y0 + t*y1
+ // z = z0 + t*z1
+
+ HPoint3D point;
+ HPoint3D vector;
+
+ float t;
+
+ };
+
+
+ GeoUtils ();
+ HPoint3D getVector (HPoint3D p0, HPoint3D p1);
+
+ private:
+
+ };
+
+
+
+}
Added: trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.cfg
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.cfg (rev 0)
+++ trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.cfg 2013-08-12 10:53:01 UTC (rev 970)
@@ -0,0 +1,2 @@
+rgbdCalibrator.CameraColor.Proxy=cameraA:default -h localhost -p 9998
+rgbdCalibrator.CameraDepth.Proxy=cameraB:default -h localhost -p 9998
Added: trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.cpp
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.cpp (rev 0)
+++ trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.cpp 2013-08-12 10:53:01 UTC (rev 970)
@@ -0,0 +1,106 @@
+/*
+ *
+ * Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ *
+ * Authors : Roberto Calvo <rocapal [at] gsyc [dot] urjc [dot] es>
+ *
+ */
+
+#include <iostream>
+#include <Ice/Ice.h>
+#include <IceUtil/IceUtil.h>
+#include <jderobot/camera.h>
+#include <colorspaces/colorspacesmm.h>
+#include "viewer.h"
+#include <cv.h>
+#include <highgui.h>
+
+
+
+int main(int argc, char** argv){
+ 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";
+
+ /*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";
+
+ /*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";
+
+ colorspaces::Image imgColor(data->description->width,
+ data->description->height,
+ fmt,
+ &(data->pixelData[0]));
+
+
+
+ jderobot::ImageDataPtr data2 = cprxDepth->getImageData();
+ colorspaces::Image::FormatPtr fmt2 = colorspaces::Image::Format::searchFormat(data2->description->format);
+ if (!fmt2)
+ throw "Format not supported";
+
+ colorspaces::Image imgDepth(data2->description->width,
+ data2->description->height,
+ fmt2,
+ &(data2->pixelData[0]));
+
+ viewer.setDepth(cprxDepth->getImageData());
+ 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;
+ }
+
+ if (ic)
+ ic->destroy();
+ return status;
+}
+
+
Added: trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.glade
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.glade (rev 0)
+++ trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.glade 2013-08-12 10:53:01 UTC (rev 970)
@@ -0,0 +1,256 @@
+<?xml version="1.0"?>
+<glade-interface>
+ <!-- interface-requires gtk+ 2.6 -->
+ <!-- interface-naming-policy toplevel-contextual -->
+ <widget class="GtkWindow" id="mainwindow">
+ <property name="visible">True</property>
+ <property name="title" translatable="yes">rgbdCalibrator </property>
+ <child>
+ <widget class="GtkFixed" id="fixed1">
+ <property name="visible">True</property>
+ <child>
+ <widget class="GtkLabel" id="fpslabel">
+ <property name="width_request">66</property>
+ <property name="height_request">21</property>
+ <property name="visible">True</property>
+ <property name="justify">fill</property>
+ <property name="wrap">True</property>
+ </widget>
+ <packing>
+ <property name="x">30</property>
+ <property name="y">267</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkImage" id="color_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">27</property>
+ <property name="y">21</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkImage" id="depth_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">28</property>
+ <property name="y">306</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkExpander" id="expander1">
+ <property name="width_request">264</property>
+ <property name="height_request">209</property>
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="border_width">5</property>
+ <child>
+ <widget class="GtkFixed" id="fixed2">
+ <property name="visible">True</property>
+ <child>
+ <widget class="GtkLabel" id="label2">
+ <property name="width_request">120</property>
+ <property name="height_request">25</property>
+ <property name="visible">True</property>
+ <property name="label" translatable="yes">Delay pictures:</property>
+ <property name="wrap">True</property>
+ <property name="ellipsize">end</property>
+ </widget>
+ <packing>
+ <property name="x">13</property>
+ <property name="y">53</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkEntry" id="et_sleep_photo">
+ <property name="width_request">30</property>
+ <property name="height_request">20</property>
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="invisible_char">●</property>
+ <property name="text" translatable="yes">3</property>
+ </widget>
+ <packing>
+ <property name="x">171</property>
+ <property name="y">57</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkLabel" id="label5">
+ <property name="width_request">150</property>
+ <property name="height_request">25</property>
+ <property name="visible">True</property>
+ <property name="label" translatable="yes">BorderSize Width:</property>
+ <property name="wrap">True</property>
+ </widget>
+ <packing>
+ <property name="x">18</property>
+ <property name="y">83</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkLabel" id="label1">
+ <property name="width_request">150</property>
+ <property name="height_request">25</property>
+ <property name="visible">True</property>
+ <property name="label" translatable="yes">BorderSize Height:</property>
+ <property name="wrap">True</property>
+ </widget>
+ <packing>
+ <property name="x">18</property>
+ <property name="y">112</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkEntry" id="bs_width">
+ <property name="width_request">30</property>
+ <property name="height_request">20</property>
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="invisible_char">●</property>
+ <property name="text" translatable="yes">9</property>
+ </widget>
+ <packing>
+ <property name="x">171</property>
+ <property name="y">86</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkEntry" id="bs_height">
+ <property name="width_request">30</property>
+ <property name="height_request">20</property>
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="invisible_char">●</property>
+ <property name="text" translatable="yes">6</property>
+ </widget>
+ <packing>
+ <property name="x">171</property>
+ <property name="y">112</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkLabel" id="label3">
+ <property name="width_request">150</property>
+ <property name="height_request">25</property>
+ <property name="visible">True</property>
+ <property name="label" translatable="yes">Size square</property>
+ <property name="wrap">True</property>
+ </widget>
+ <packing>
+ <property name="x">18</property>
+ <property name="y">142</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkEntry" id="bs_square_size">
+ <property name="width_request">30</property>
+ <property name="height_request">20</property>
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="invisible_char">●</property>
+ <property name="text" translatable="yes">20</property>
+ </widget>
+ <packing>
+ <property name="x">171</property>
+ <property name="y">142</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkLabel" id="label4">
+ <property name="width_request">120</property>
+ <property name="height_request">25</property>
+ <property name="visible">True</property>
+ <property name="label" translatable="yes">Num pictures:</property>
+ <property name="wrap">True</property>
+ <property name="ellipsize">end</property>
+ </widget>
+ <packing>
+ <property name="x">11</property>
+ <property name="y">24</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkEntry" id="et_num_photo">
+ <property name="width_request">30</property>
+ <property name="height_request">20</property>
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="invisible_char">●</property>
+ <property name="text" translatable="yes">8</property>
+ </widget>
+ <packing>
+ <property name="x">171</property>
+ <property name="y">29</property>
+ </packing>
+ </child>
+ </widget>
+ </child>
+ <child>
+ <widget class="GtkLabel" id="label6">
+ <property name="visible">True</property>
+ <property name="label" translatable="yes">Intrinsics Calibration Config</property>
+ </widget>
+ <packing>
+ <property name="type">label_item</property>
+ </packing>
+ </child>
+ </widget>
+ <packing>
+ <property name="x">650</property>
+ <property name="y">24</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkButton" id="bt_take_photo">
+ <property name="label" translatable="yes">1. TakePictures</property>
+ <property name="width_request">133</property>
+ <property name="height_request">40</property>
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="receives_default">True</property>
+ </widget>
+ <packing>
+ <property name="x">435</property>
+ <property name="y">46</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkButton" id="bt_intrinsic_calib">
+ <property name="label" translatable="yes">2. Intrinsic Calibration</property>
+ <property name="width_request">172</property>
+ <property name="height_request">40</property>
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="receives_default">True</property>
+ </widget>
+ <packing>
+ <property name="x">414</property>
+ <property name="y">94</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkTextView" id="tv_status">
+ <property name="width_request">255</property>
+ <property name="height_request">112</property>
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="text" translatable="yes">Info</property>
+ </widget>
+ <packing>
+ <property name="x">381</property>
+ <property name="y">147</property>
+ </packing>
+ </child>
+ </widget>
+ </child>
+ </widget>
+</glade-interface>
Added: trunk/src/stable/components/rgbdCalibrator/viewer.cpp
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/viewer.cpp (rev 0)
+++ trunk/src/stable/components/rgbdCalibrator/viewer.cpp 2013-08-12 10:53:01 UTC (rev 970)
@@ -0,0 +1,294 @@
+/*
+ *
+ * Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ *
+ * Authors : Roberto Calvo <rocapal [at] gsyc [dot] urjc [dot] es>
+ *
+ */
+
+#include "viewer.h"
+#include <iostream>
+#include <Ice/Ice.h>
+#include <IceUtil/IceUtil.h>
+#include <boost/filesystem.hpp>
+#include "calibration.h"
+
+using namespace boost::filesystem;
+using namespace cv;
+
+namespace rgbdCalibrator{
+
+
+ const std::string gladepath = std::string(GLADE_DIR) +
+ std::string("/rgbdCalibrator.glade");
+
+ const std::string pathImage = "./images/";
+
+
+ Viewer::Viewer()
+ : gtkmain(0,0),frameCount(0),
+ intrinsicsEnable(0),contPhoto(1) {
+
+ std::cout << "Loading glade\n";
+
+ // ref widgets
+ refXml = Gnome::Glade::Xml::create(gladepath);
+ refXml->get_widget("color_image", gtkimage_color);
+ refXml->get_widget("depth_image", gtkimage_depth);
+ refXml->get_widget("mainwindow",mainwindow);
+ refXml->get_widget("fpslabel",fpsLabel);
+ refXml->get_widget("bt_take_photo", btTakePhoto);
+ refXml->get_widget("et_sleep_photo", etSleepPhoto);
+ refXml->get_widget("et_num_photo", etNumPhoto);
+ refXml->get_widget("tv_status", tvStatus);
+ refXml->get_widget("bt_intrinsic_calib", btIntrinsic);
+
+ // connect signals
+ btTakePhoto->signal_clicked().connect(sigc::mem_fun(this,&Viewer::on_bt_take_photo_clicked));
+
+ btIntrinsic->signal_clicked().connect(sigc::mem_fun(this,&Viewer::on_bt_intrinsic));
+
+
+ // start the timer for calculating the number of frames per second
+ // the images are being displayed at
+ oldFrameTime = IceUtil::Time::now();
+
+
+ }
+
+
+ Viewer::~Viewer() {}
+
+ bool Viewer::isVisible(){
+ return mainwindow->is_visible();
+ }
+
+ void Viewer::display( const colorspaces::Image& imageColor, const colorspaces::Image& imageDepth )
+ {
+ 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,
+ Gdk::COLORSPACE_RGB,
+ false,
+ 8,
+ img_rgb8.width,
+ img_rgb8.height,
+ img_rgb8.step);
+
+ gtkimage_color->clear();
+ gtkimage_color->set(imgBuffColor);
+
+ if (intrinsicsEnable)
+ saveImage(imageColor);
+
+
+ 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);
+
+
+
+ displayFrameRate();
+ mainwindow->resize(1,1);
+ while (gtkmain.events_pending())
+ gtkmain.iteration();
+ }
+
+ void Viewer::setDepth(const jderobot::ImageDataPtr depth)
+ {
+ dataDepth = depth;
+ }
+
+ void
+ Viewer::displayFrameRate()
+ {
+ double diff;
+ IceUtil::Time diffT;
+
+ currentFrameTime = IceUtil::Time::now();
+ diff = (currentFrameTime - oldFrameTime).toMilliSecondsDouble();
+ if (diff < 1000.0)
+ frameCount++;
+ else{
+ oldFrameTime = currentFrameTime;
+ fps = frameCount*1000.0/diff;
+ frameCount=0;
+ // Display the frame rate
+ std::stringstream fpsString;
+ fpsString << "fps = " << int(fps);
+ fpsLabel->set_label(fpsString.str());
+ }
+ }
+
+ void Viewer::on_bt_take_photo_clicked()
+ {
+ intrinsicsEnable = 1;
+
+ // Get num of photos
+ std::string num (etNumPhoto->get_buffer()->get_text());
+ numPhoto = atoi(num.c_str());
+ std::cout << numPhoto << std::endl;
+
+ // Get delay photo and Init time last photo
+ std::string sleep (etSleepPhoto->get_buffer()->get_text());
+ delayPhoto = atoi(sleep.c_str());
+ lastTimePhoto = IceUtil::Time::now();
+
+ tvStatus->get_buffer()->set_text("Init process, show the pattern!");
+ }
+
+ void Viewer::saveImage(const colorspaces::Image& imageColor)
+ {
+
+
+ IceUtil::Time currentTime = IceUtil::Time::now();
+ if ((currentTime - lastTimePhoto).toSeconds() == delayPhoto)
+ {
+
+ // Check the directory
+ if ( !exists(pathImage))
+ {
+ path dir(pathImage);
+ if (!boost::filesystem::create_directory(dir))
+ std::cout << "Error to create directory" << std::endl;
+ }
+
+
+ lastTimePhoto = IceUtil::Time::now();
+
+ // Convert to gray
+ Mat gray(imageColor.size(), CV_8UC1);
+ cvtColor(imageColor, gray, CV_RGB2GRAY);
+
+ // Save Image
+ std::stringstream filename;
+ filename << pathImage << "img" << contPhoto << ".jpg";
+ imwrite( filename.str().c_str(), gray );
+
+ std::string msg = "Image saved: " + filename.str();
+ tvStatus->get_buffer()->set_text(msg.c_str());
+
+ beep();
+
+ if (contPhoto < numPhoto)
+ contPhoto ++;
+ else
+ {
+ intrinsicsEnable = 0;
+ contPhoto = 1;
+ }
+ }
+
+ }
+
+ void Viewer::beep()
+ {
+ system("mplayer beep.wav &> /dev/null");
+ }
+
+ void Viewer::on_bt_intrinsic()
+ {
+
+
+ tvStatus->get_buffer()->set_text("Getting intrinsics ...");
+
+ Size boardSize, imageSize;
+ Mat cameraMatrix, distCoeffs;
+ vector<vector<Point2f> > imagePoints;
+
+
+ // TODO: get this data from user by GUI
+ boardSize.width = 9;
+ boardSize.height= 6;
+
+ //flag
+ int flag = 0;
+ flag |= CV_CALIB_FIX_PRINCIPAL_POINT;
+ flag |= CV_CALIB_ZERO_TANGENT_DIST;
+ flag |= CV_CALIB_FIX_ASPECT_RATIO;
+
+ // List of images
+ directory_iterator end_itr;
+ for ( directory_iterator itr( pathImage ); itr != end_itr; ++itr )
+ {
+
+ Mat view;
+ view = imread(itr->path().c_str(), CV_LOAD_IMAGE_COLOR);
+
+ imageSize = view.size();
+
+ vector<Point2f> pointBuf;
+ bool found = findChessboardCorners( view, boardSize, pointBuf,
+ CV_CALIB_CB_ADAPTIVE_THRESH |
+ CV_CALIB_CB_FAST_CHECK |
+ CV_CALIB_CB_NORMALIZE_IMAGE);
+
+ if (found)
+ {
+
+ Mat viewGray;
+ cvtColor(view, viewGray, CV_BGR2GRAY);
+ cornerSubPix( viewGray, pointBuf, Size(11,11),
+ Size(-1,-1), TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
+
+ imagePoints.push_back(pointBuf);
+
+ // Draw the corners.
+ drawChessboardCorners( view, boardSize, Mat(pointBuf), found );
+ //imshow("Image View", view);
+
+ }
+ }
+
+ Calibration* calib = new Calibration();
+ calib->runCalibrationAndSave(boardSize, 20.0, flag, imageSize,
+ cameraMatrix, distCoeffs, imagePoints);
+
+ std::cout << std::endl << cameraMatrix << std::endl;
+
+ std::stringstream matrixStr;
+
+ matrixStr << "Intrinsic Matrix" << std::endl;
+ matrixStr << "================" << std::endl << std::endl;
+
+ for (int i=0; i<cameraMatrix.cols ; i++)
+ {
+ for (int j=0; j<cameraMatrix.rows; j++)
+ {
+ matrixStr << cameraMatrix.at<double>(i,j) << "\t\t";
+ if (i==2 && j==0)
+ matrixStr << "\t";
+ }
+ matrixStr<<std::endl;
+
+ tvStatus->get_buffer()->set_text(matrixStr.str().c_str());
+ }
+
+ calib->extrinsics(cameraMatrix, dataDepth);
+
+ }
+
+
+}//namespace
Added: trunk/src/stable/components/rgbdCalibrator/viewer.h
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/viewer.h (rev 0)
+++ trunk/src/stable/components/rgbdCalibrator/viewer.h 2013-08-12 10:53:01 UTC (rev 970)
@@ -0,0 +1,99 @@
+/*
+ *
+ * Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ *
+ * Authors : Roberto Calvo <rocapal [at] gsyc [dot] urjc [dot] es>
+ *
+ */
+
+#ifndef RGBDCALIBRATOR_VIEWER_H
+#define RGBDCALIBRATOR_VIEWER_H
+
+
+#include <gtkmm.h>
+#include <libglademm.h>
+#include <IceUtil/Thread.h>
+#include <IceUtil/Time.h>
+#include <string>
+#include <colorspaces/colorspacesmm.h>
+#include <cmath>
+#include <jderobot/camera.h>
+
+using namespace cv;
+
+namespace rgbdCalibrator{
+
+
+ class Viewer
+ {
+ public:
+ Viewer();
+ ~Viewer();
+
+ bool isVisible();
+
+ //! function that actually displays the image in a window
+ void display( const colorspaces::Image& imageColor, const colorspaces::Image& imageDepth );
+
+ void setDepth(const jderobot::ImageDataPtr depth);
+
+ private:
+ Glib::RefPtr<Gnome::Glade::Xml> refXml;
+ Gtk::Image* gtkimage_color;
+ Gtk::Image* gtkimage_depth;
+ Gtk::Window* mainwindow;
+ Gtk::Label* fpsLabel;
+ Gtk::Label* lbSleepPhoto;
+ Gtk::Button* btTakePhoto;
+ Gtk::Button* btIntrinsic;
+ Gtk::Entry* etSleepPhoto;
+ Gtk::Entry* etNumPhoto;
+ Gtk::TextView* tvStatus;
+ Gtk::Main gtkmain;
+
+ //! display the frame rate of the received images
+ void displayFrameRate();
+
+ //! time variables for calculating number of frames per second
+ IceUtil::Time currentFrameTime,oldFrameTime;
+ double fps;
+ int frameCount;
+
+ // Intrinsics variables
+ int intrinsicsEnable;
+ IceUtil::Time lastTimePhoto;
+ int delayPhoto;
+ int numPhoto;
+ int contPhoto;
+
+ jderobot::ImageDataPtr dataDepth;
+
+
+ // onclicks
+ void on_bt_take_photo_clicked ();
+ void on_bt_intrinsic();
+
+ void saveImage(const colorspaces::Image& imageColor);
+ void beep();
+
+
+
+
+ };
+
+}//namespace
+
+#endif //RGBDCALIBRATOR_VIEWER_H
More information about the Jderobot-admin
mailing list