diff --git a/CMakeLists.txt b/CMakeLists.txt index 873e3c4..b7e6344 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,4 +3,4 @@ cmake_minimum_required(VERSION 2.6) find_package(Rock) rock_init(qcam_calib 0.1) -rock_standard_layout() +rock_standard_layout() \ No newline at end of file diff --git a/src/ItemsStereoCamera.cpp b/src/ItemsStereoCamera.cpp new file mode 100644 index 0000000..b51aadd --- /dev/null +++ b/src/ItemsStereoCamera.cpp @@ -0,0 +1,426 @@ +/* + * ItensStereoCamera.cpp + * + * Created on: Jan 4, 2016 + * Author: tiagotrocoli + */ + +#include "ItemsStereoCamera.hpp" + +//#include +#include +#include +#include + +#include +#include +#include + +using namespace qcam_calib; + +cv::Mat fillMatrixParametersbyParametersValues(StereoCameraParameterItem* items, cv::Size sizeMat, QList parameters) { + + cv::Mat mat = cv::Mat(sizeMat, CV_64FC1); + cv::MatIterator_ it = mat.begin(); + int i = 0; + while (it != mat.end()) { + (*it) = items->getParameter(parameters[i]); + ++it; + ++i; + } + + return mat; +} + +void fillIntrinsecMatrixFromParametersValues(StereoCameraParameterItem* items, cv::Mat &intrinsic, cv::Mat &distCoeff, QList parameters) { + + cv::Mat k = cv::Mat::zeros(3, 3, CV_64FC1); + cv::Mat dist = cv::Mat::zeros(8, 1, CV_64FC1); + + k.at(0, 0) = items->getParameter(parameters[0]); + k.at(1, 1) = items->getParameter(parameters[1]); + k.at(0, 2) = items->getParameter(parameters[2]); + k.at(1, 2) = items->getParameter(parameters[3]); + + dist.at(0) = items->getParameter(parameters[3]); + dist.at(1) = items->getParameter(parameters[4]); + dist.at(2) = items->getParameter(parameters[5]); + dist.at(3) = items->getParameter(parameters[6]); + dist.at(4) = items->getParameter(parameters[7]); + dist.at(5) = items->getParameter(parameters[8]); + dist.at(6) = items->getParameter(parameters[9]); + dist.at(7) = items->getParameter(parameters[10]); + dist.at(8) = items->getParameter(parameters[11]); + + k.copyTo(intrinsic); + dist.copyTo(distCoeff); +} + +void fillParametersValuesbyListParameters(StereoCameraParameterItem* items, cv::Mat mat, QList parameters) { + + cv::MatIterator_ it = mat.begin(); + int i = 0; + while (it != mat.end()) { + items->setParameter(parameters[i], (*it)); + ++it; + ++i; + } +} + +void fillParametersValuesFromIntrinsecMatrix(StereoCameraParameterItem* items, cv::Mat intrinsic, cv::Mat distMatrix, QList parameters) { + + items->setParameter(parameters[0], intrinsic.at(0, 0)); // fx + items->setParameter(parameters[1], intrinsic.at(1, 1)); // fy + items->setParameter(parameters[2], intrinsic.at(0, 2)); // cx + items->setParameter(parameters[3], intrinsic.at(1, 2)); // cx + + items->setParameter(parameters[4], distMatrix.at(0)); // k1 + items->setParameter(parameters[5], distMatrix.at(1)); // k2 + items->setParameter(parameters[6], distMatrix.at(2)); // p1 + items->setParameter(parameters[7], distMatrix.at(3)); // p2 + items->setParameter(parameters[8], distMatrix.at(4)); // k3 + items->setParameter(parameters[9], distMatrix.at(5)); // k4 + items->setParameter(parameters[10], distMatrix.at(6)); // k5 + items->setParameter(parameters[11], distMatrix.at(7)); // k6 +} + +StereoCameraParameterItem::StereoCameraParameterItem(const QString& string, QList parameters) : + QCamCalibItem(string) { + + setEditable(false); + setColumnCount(2); + + for (int i = 0; i < parameters.size(); ++i) { + setParameter(parameters[i], 0); + } +} + +void StereoCameraParameterItem::setParameter(const QString& name, double val) { + QModelIndexList list; + if (model() && index().isValid()) + list = model()->match(index().child(0, 0), Qt::DisplayRole, QVariant(name), 1, Qt::MatchExactly); + if (list.empty()) { + QList items; + items.append(new QCamCalibItem(name)); + items.back()->setEditable(false); + items.append(new QCamCalibItem(val)); + items.back()->setEditable(false); + appendRow(items); + } else { + QStandardItem *item = child(list.front().row(), 1); + if (item) + item->setText(QString::number(val)); + } +} + +double StereoCameraParameterItem::getParameter(const QString& name) const { + QModelIndexList list; + if (model() && index().isValid()) + list = model()->match(index().child(0, 0), Qt::DisplayRole, QVariant(name), 1, Qt::MatchExactly); + if (!list.empty()) { + QStandardItem *item = child(list.front().row(), 1); + if (item) + return item->data(Qt::EditRole).toDouble(); + } else + throw std::runtime_error("Cannot find parameter!"); + return 0; +} + +//StereoImageItem +StereoImageItem::StereoImageItem(const QString& name, const QString& path) : + chessboard(NULL), QCamCalibItem(name), image_path(path) { +} + +StereoImageItem::~StereoImageItem() { +} + +bool StereoImageItem::isChessboardFound() { + return &this->chessboard != 0 && this->chessboard.size() > 0; +} + +const QString& StereoImageItem::getImagePath() const { + return this->image_path; +} + +void StereoImageItem::setChessboardCorners(QVector points) { + this->chessboard = points; +} + +const QVector& StereoImageItem::getChessboardCorners() const { + return this->chessboard; +} + +QImage qcam_calib::StereoImageItem::getImageWithChessboard(int cols, int rows) { + + QImage qt_image(this->image_path); + qt_image = qt_image.convertToFormat(QImage::Format_RGB888); + cv::Mat cv_image(qt_image.height(), qt_image.width(), CV_8UC3, qt_image.bits(), qt_image.bytesPerLine()); + + std::vector cv_points; + QVector qt_points = this->chessboard; + + if (this->isChessboardFound()) { + cv_points = StereoTools::convertQVectorQPointFToVectorPoints2f(this->chessboard); + cv::drawChessboardCorners(cv_image, cv::Size(cols, rows), cv_points, true); + } + + + return QImage(cv_image.data, cv_image.cols, cv_image.rows, cv_image.step, QImage::Format_RGB888).copy(); +} + +//StereoCameraItem +StereoCameraItem::StereoCameraItem(int id, const QString& string) : + QCamCalibItem(string), camera_id(id) { + + setEditable(false); + this->parameter = new StereoCameraParameterItem("Parameters", INTRINSIC_PARAMETERS_LIST); + appendRow(this->parameter); + + images = new QStandardItem("Images"); + images->setEditable(false); + appendRow(images); +} + +int StereoCameraItem::getId() { + return this->camera_id; +} + +StereoImageItem* StereoCameraItem::addImages(const QList &stereoImageitems) { + images->appendRow(stereoImageitems); +} + +QStandardItem* StereoCameraItem::getImagesItems() const { + return this->images; +} + +StereoCameraParameterItem* StereoCameraItem::getParameter() const { + return this->parameter; +} + +//StereoItem +StereoItem::StereoItem(int id, const QString& string) : + QCamCalibItem(string), stereo_id(id) { + + setEditable(false); + + this->parameters = new QStandardItem("Parameters"); + this->parameters->setEditable(false); + appendRow(this->parameters); + + this->parameters->appendRow(new StereoCameraParameterItem("Fundamental Matrix", FUNDAMENTAL_MATRIX_PARAMETERS_LIST)); + this->parameters->appendRow(new StereoCameraParameterItem("Essential Matrix", FUNDAMENTAL_MATRIX_PARAMETERS_LIST)); + this->parameters->appendRow(new StereoCameraParameterItem("Rotation Matrix", ROTATION_MATRIX_PARAMETERS_LIST)); + this->parameters->appendRow(new StereoCameraParameterItem("Translation Matrix", TRANSLATION_VECTOR_PARAMETERS_LIST)); + + this->error_values = new StereoCameraParameterItem("Error Values", ERROR_VALUES_PARAMETERS_LIST); + appendRow(this->error_values); + + this->left_camera = new StereoCameraItem(0, "Left Camera"); + appendRow(this->left_camera); + + this->right_camera = new StereoCameraItem(1, "Right Camera"); + appendRow(this->right_camera); +} + +int StereoItem::getId() { + return this->stereo_id; +} + +char* StereoItem::getBaseName() { + return "Stereo Camera "; +} + +void StereoItem::calibrate(int cols, int rows, float dx, float dy) { + + QStandardItem* left_images = this->left_camera->getImagesItems(); + QStandardItem* right_images = this->right_camera->getImagesItems(); + + int item_row_count = left_images->rowCount(); + if (left_images->rowCount() > right_images->rowCount()) + item_row_count = right_images->rowCount(); + + std::vector > object_points; + std::vector > left_points, right_points; + + std::vector chessboard_3Dpoints; + for (int row = 0; row < rows; ++row) + for (int col = 0; col < cols; ++col) + chessboard_3Dpoints.push_back(cv::Point3f(dx * col, dy * row, 0)); + + int j=0; + int image_sample = -1; + for (int i = 0; i < item_row_count; ++i) { + StereoImageItem* left_stereo_image = dynamic_cast(left_images->child(i, 0)); + StereoImageItem* right_stereo_image = dynamic_cast(right_images->child(i, 0)); + + bool both_chessboard_detect = left_stereo_image->isChessboardFound() && right_stereo_image->isChessboardFound(); + bool equal_number_of_points_left = left_stereo_image->getChessboardCorners().size() == cols*rows; + bool equal_number_of_points_right = right_stereo_image->getChessboardCorners().size() == cols*rows; + + if (!both_chessboard_detect || !equal_number_of_points_left || !equal_number_of_points_right) + continue; + + left_points.push_back(StereoTools::convertQVectorQPointFToVectorPoints2f(left_stereo_image->getChessboardCorners())); + right_points.push_back(StereoTools::convertQVectorQPointFToVectorPoints2f(right_stereo_image->getChessboardCorners())); + object_points.push_back(chessboard_3Dpoints); + image_sample = i; + } + + if (object_points.size() < 5) + throw std::runtime_error("Not enough detected chessboards!"); + + StereoImageItem* stereo_image = dynamic_cast(left_images->child(image_sample, 0)); + QImage qt_image(stereo_image->getImagePath()); + qt_image = qt_image.convertToFormat(QImage::Format_RGB888); + cv::Size image_size(qt_image.height(), qt_image.width()); + + std::vector vec_matrix = StereoTools::stereoCalibrate(left_points, right_points, object_points, image_size); + + fillParametersValuesFromIntrinsecMatrix(this->left_camera->getParameter(), vec_matrix[0], vec_matrix[1], INTRINSIC_PARAMETERS_LIST); + fillParametersValuesFromIntrinsecMatrix(this->right_camera->getParameter(), vec_matrix[2], vec_matrix[3], INTRINSIC_PARAMETERS_LIST); + + this->error_values->setParameter(ERROR_VALUES_PARAMETERS_LIST[0], vec_matrix[4].at(0, 0)); + this->error_values->setParameter(ERROR_VALUES_PARAMETERS_LIST[1], vec_matrix[4].at(0, 1)); + + fillParametersValuesbyListParameters(dynamic_cast(this->parameters->child(0, 0)), vec_matrix[5], FUNDAMENTAL_MATRIX_PARAMETERS_LIST); + fillParametersValuesbyListParameters(dynamic_cast(this->parameters->child(1, 0)), vec_matrix[6], FUNDAMENTAL_MATRIX_PARAMETERS_LIST); + fillParametersValuesbyListParameters(dynamic_cast(this->parameters->child(2, 0)), vec_matrix[7], ROTATION_MATRIX_PARAMETERS_LIST); + fillParametersValuesbyListParameters(dynamic_cast(this->parameters->child(3, 0)), vec_matrix[8], TRANSLATION_VECTOR_PARAMETERS_LIST); + +} + +void StereoItem::saveParameter(const QString& path) const { + + std::vector intrisc_mat(2), dist_coeff(2); + fillIntrinsecMatrixFromParametersValues(this->left_camera->getParameter(), intrisc_mat[0], dist_coeff[0], INTRINSIC_PARAMETERS_LIST); + fillIntrinsecMatrixFromParametersValues(this->right_camera->getParameter(), intrisc_mat[1], dist_coeff[1], INTRINSIC_PARAMETERS_LIST); + + cv::Mat fundamental = fillMatrixParametersbyParametersValues(dynamic_cast(parameters->child(0, 0)), cv::Size(3, 3), FUNDAMENTAL_MATRIX_PARAMETERS_LIST); + cv::Mat essential = fillMatrixParametersbyParametersValues(dynamic_cast(parameters->child(1, 0)), cv::Size(3, 3), FUNDAMENTAL_MATRIX_PARAMETERS_LIST); + cv::Mat rotation = fillMatrixParametersbyParametersValues(dynamic_cast(parameters->child(2, 0)), cv::Size(3, 3), ROTATION_MATRIX_PARAMETERS_LIST); + cv::Mat translation = fillMatrixParametersbyParametersValues(dynamic_cast(parameters->child(3, 0)), cv::Size(3, 1), TRANSLATION_VECTOR_PARAMETERS_LIST); + + cv::FileStorage fs(path.toStdString(), cv::FileStorage::WRITE); + time_t rawtime; + time(&rawtime); + fs << "calibrationDate" << asctime(localtime(&rawtime)); + fs << "leftCameraMatrix" << intrisc_mat[0] << "leftDistCoeffs" << dist_coeff[0]; + fs << "rightCameraMatrix" << intrisc_mat[1] << "rightDistCoeffs" << dist_coeff[1]; + fs << "fundamentalMatrix" << fundamental << "essentialMatrix" << essential; + fs << "rotationMatrix" << rotation << "translationMatrix" << translation; + fs.release(); +} + +bool StereoItem::isCalibrated() { + return error_values->getParameter(ERROR_VALUES_PARAMETERS_LIST[0]) > 0; +} + +QList StereoTools::loadStereoImageAndFindChessboardItem(const QString& path, int cols, int rows) { + + QList items = loadStereoImageItem(path); + + StereoImageItem *item = dynamic_cast(items.at(0)); + item->setChessboardCorners(findChessboard(path, cols, rows)); + + if (item->isChessboardFound()) + items.at(1)->setText("OK, Chessboard Found"); + + return items; +} + +QList StereoTools::loadStereoImageItem(const QString &path) { + + QFileInfo info(path); + + QList items; + StereoImageItem *item = new StereoImageItem(info.fileName(), info.absoluteFilePath()); + + item->setEditable(false); + items.append(item); + + items.append(new QCamCalibItem("No Chessboard")); + items.back()->setEditable(false); + + return items; +} + +QVector StereoTools::findChessboard(const QString &path, int cols, int rows) { + + QImage qt_image(path); + qt_image = qt_image.convertToFormat(QImage::Format_RGB888); + cv::Mat cv_image(qt_image.height(), qt_image.width(), CV_8UC3, qt_image.bits(), qt_image.bytesPerLine()); + cv::cvtColor(cv_image, cv_image, cv::COLOR_BGR2GRAY); + std::vector cv_points; + bool found_chessboard = cv::findChessboardCorners(cv_image, cv::Size(cols, rows), cv_points); + + QVector qt_points; + if(found_chessboard) + qt_points = convertVectorPoints2fToQVectorQPointF(cv_points); + + return qt_points; +} + +std::vector StereoTools::stereoCalibrate(std::vector > left_points, std::vector > right_points, + std::vector > object_points, cv::Size image_size) { + + cv::Mat camera_matrix[2], dist_coeffs[2]; + camera_matrix[0] = cv::Mat::eye(3, 3, CV_64F); + camera_matrix[1] = cv::Mat::eye(3, 3, CV_64F); + cv::Mat rotation, translation, essential, fundamental; + + double rms = cv::stereoCalibrate(object_points, left_points, right_points, camera_matrix[0], dist_coeffs[0], camera_matrix[1], dist_coeffs[1], image_size, rotation, translation, essential, + fundamental, cv::TermCriteria(CV_TERMCRIT_ITER + CV_TERMCRIT_EPS, 1000, 1e-8), cv::CALIB_RATIONAL_MODEL); + + double err = 0; + int npoints = 0; + std::vector lines[2]; + for (int i = 0; i < object_points.size(); i++) { + int npt = (int) left_points[i].size(); + cv::Mat imgpt[2]; + for (int k = 0; k < 2; k++) { + std::vector tempPoints; + k == 0 ? tempPoints = left_points[i] : tempPoints = right_points[i]; + imgpt[k] = cv::Mat(tempPoints); + cv::undistortPoints(imgpt[k], imgpt[k], camera_matrix[k], dist_coeffs[k], cv::Mat(), camera_matrix[k]); + cv::computeCorrespondEpilines(imgpt[k], k + 1, fundamental, lines[k]); + } + for (int j = 0; j < npt; j++) { + double errij = fabs(left_points[i][j].x * lines[1][j][0] + left_points[i][j].y * lines[1][j][1] + lines[1][j][2]) + + fabs(right_points[i][j].x * lines[0][j][0] + right_points[i][j].y * lines[0][j][1] + lines[0][j][2]); + err += errij; + } + npoints += npt; + } + + cv::Mat error_mat = (cv::Mat_(1, 2) << rms, err / npoints); + + std::vector mats; + mats.push_back(camera_matrix[0]); + mats.push_back(dist_coeffs[0]); + mats.push_back(camera_matrix[1]); + mats.push_back(dist_coeffs[1]); + + mats.push_back(error_mat); + mats.push_back(fundamental); + mats.push_back(essential); + mats.push_back(rotation); + mats.push_back(translation); + + return mats; +} + +QVector StereoTools::convertVectorPoints2fToQVectorQPointF(const std::vector&points1) { + QVector points2; + std::vector::const_iterator iter = points1.begin(); + for (; iter != points1.end(); ++iter) + points2.push_back(QPointF(iter->x, iter->y)); + return points2; +} + +std::vector StereoTools::convertQVectorQPointFToVectorPoints2f(const QVector&points1) { + std::vector points2; + QVector::const_iterator iter = points1.begin(); + for (; iter != points1.end(); ++iter) + points2.push_back(cv::Point2f(iter->x(), iter->y())); + return points2; +} diff --git a/src/ItemsStereoCamera.hpp b/src/ItemsStereoCamera.hpp new file mode 100644 index 0000000..915d885 --- /dev/null +++ b/src/ItemsStereoCamera.hpp @@ -0,0 +1,109 @@ +/* + * ItensStereoCamera.h + * + * Created on: Jan 4, 2016 + * Author: tiagotrocoli + */ + +#ifndef GUI_QCAM_CALIB_SRC_ITEMSSTEREOCAMERA_HPP_ +#define GUI_QCAM_CALIB_SRC_ITEMSSTEREOCAMERA_HPP_ + +#include +#include +#include + +#include + +#include + +#include "Items.hpp" + +namespace qcam_calib { + +static const QList INTRINSIC_PARAMETERS_LIST = QList() << "fx" << "fy" << "cx" << "cy" << "k1" << "k2" << "p1" << "p2"<<"k3"<<"k4"<<"k5"<<"k6"; +static const QList FUNDAMENTAL_MATRIX_PARAMETERS_LIST = QList() << "e1" << "e2" << "e3" << "e4" << "e5" << "e6" << "e7" << "e8" << "e9"; +static const QList ROTATION_MATRIX_PARAMETERS_LIST = QList() << "r1" << "r2" << "r3" << "r4" << "r5" << "r6" << "r7" << "r8" << "r9"; +static const QList TRANSLATION_VECTOR_PARAMETERS_LIST = QList() << "t1" << "t2" << "t3"; +static const QList ERROR_VALUES_PARAMETERS_LIST = QList() << "RMS Error" << "Average reprojection error"; + + +class StereoCameraParameterItem: public QCamCalibItem { +public: + StereoCameraParameterItem(const QString &string, QList parameters); + void setParameter(const QString &name, double val = 0); + double getParameter(const QString &name) const; + + // vou modificar + void save(const QString &path) const; + +}; + +class StereoImageItem: public QCamCalibItem { +public: + StereoImageItem(const QString &name, const QString &path); + virtual ~StereoImageItem(); + + QImage getImageWithChessboard(int cols, int rows); + + bool isChessboardFound(); + const QString &getImagePath() const; + const QVector &getChessboardCorners() const; + void setChessboardCorners(QVector points); + +private: + QString image_path; // path to image + QVector chessboard; +}; + +class StereoCameraItem: public QCamCalibItem { +public: + StereoCameraItem(int id, const QString &string); + + int getId(); + StereoImageItem* addImages(const QList &stereo_image_items); + StereoImageItem* getImageItem(const QString &name); + QStandardItem* getImagesItems() const; + StereoCameraParameterItem* getParameter() const; + +private: + int camera_id; + StereoCameraParameterItem* parameter; + QStandardItem *images; +}; + +class StereoItem: public QCamCalibItem { +public: + StereoItem(int id, const QString &string); + int getId(); + static char* getBaseName(); + + void calibrate(int cols, int rows, float dx, float dy); + void saveParameter(const QString &path) const; + bool isCalibrated(); + +private: + int stereo_id; + QStandardItem *parameters; + StereoCameraParameterItem* error_values; + + StereoCameraItem *left_camera; + StereoCameraItem *right_camera; +}; + +class StereoTools { + +public: + + static QList loadStereoImageAndFindChessboardItem(const QString& path, int cols, int rows); + static QList loadStereoImageItem(const QString &path); + static QVector findChessboard(const QString &path, int cols, int rows); + static std::vector stereoCalibrate(std::vector > left_points, std::vector > right_points, std::vector > object_points, + cv::Size image_size); + + static QVector convertVectorPoints2fToQVectorQPointF(const std::vector &points); + static std::vector convertQVectorQPointFToVectorPoints2f(const QVector &points); +}; + +} + +#endif /* GUI_QCAM_CALIB_SRC_ITEMSSTEREOCAMERA_HPP_ */ diff --git a/src/QCamCalib.cpp b/src/QCamCalib.cpp index f20c32c..e839507 100644 --- a/src/QCamCalib.cpp +++ b/src/QCamCalib.cpp @@ -1,5 +1,6 @@ #include "QCamCalib.hpp" #include "Items.hpp" +#include "ItemsStereoCamera.hpp" #include "ImageView.hpp" #include "ui_main_gui.h" @@ -16,19 +17,8 @@ using namespace qcam_calib; const char* CAMERA_BASE_NAME = "camera_"; QCamCalib::QCamCalib(QWidget *parent) : - QWidget(parent), - current_load_path("."), - tree_model(NULL), - camera_item_menu(NULL), - tree_view_menu(NULL), - image_item_menu(NULL), - progress_dialog_images(NULL), - progress_dialog_chessboard(NULL), - progress_dialog_calibrate(NULL), - future_watcher_images(NULL), - future_watcher_chessboard(NULL), - future_watcher_calibrate(NULL) -{ + QWidget(parent), current_load_path("."), tree_model(NULL), camera_item_menu(NULL), tree_view_menu(NULL), image_item_menu(NULL), progress_dialog_images(NULL), progress_dialog_chessboard(NULL), progress_dialog_calibrate( + NULL), future_watcher_images(NULL), future_watcher_chessboard(NULL), future_watcher_calibrate(NULL) { Ui::MainGui gui; gui.setupUi(this); @@ -43,154 +33,188 @@ QCamCalib::QCamCalib(QWidget *parent) : // camera item menu camera_item_menu = new QMenu(this); - QAction* act = new QAction("load images",this); - connect(act,SIGNAL(triggered()),this,SLOT(loadImages())); + QAction* act = new QAction("load images", this); + connect(act, SIGNAL(triggered()), this, SLOT(loadImages())); camera_item_menu->addAction(act); - act = new QAction("calibrate",this); - connect(act,SIGNAL(triggered()),this,SLOT(calibrateCamera())); + act = new QAction("calibrate", this); + connect(act, SIGNAL(triggered()), this, SLOT(calibrateCamera())); camera_item_menu->addAction(act); - act = new QAction("save parameter",this); - connect(act,SIGNAL(triggered()),this,SLOT(saveCameraParameter())); + act = new QAction("save parameter", this); + connect(act, SIGNAL(triggered()), this, SLOT(saveCameraParameter())); camera_item_menu->addAction(act); - QAction *act_remove = new QAction("remove",this); - connect(act_remove,SIGNAL(triggered()),this,SLOT(removeCurrentItem())); + QAction *act_remove = new QAction("Remove", this); + connect(act_remove, SIGNAL(triggered()), this, SLOT(removeCurrentItem())); camera_item_menu->addAction(act_remove); // tree view menu + // mono camera tree_view_menu = new QMenu(this); - act = new QAction("add Camera",this); - connect(act,SIGNAL(triggered()),this,SLOT(addCamera())); + act = new QAction("add Camera", this); + connect(act, SIGNAL(triggered()), this, SLOT(addCamera())); tree_view_menu->addAction(act); // image item menu image_item_menu = new QMenu(this); image_item_menu->addAction(act_remove); - act = new QAction("find chessboard",this); - connect(act,SIGNAL(triggered()),this,SLOT(findChessBoard())); + act = new QAction("find chessboard", this); + connect(act, SIGNAL(triggered()), this, SLOT(findChessBoard())); image_item_menu->addAction(act); + // tree view menu + // stereo item + act = new QAction("Add Stereo Camera", this); + connect(act, SIGNAL(triggered()), this, SLOT(addStereoItem())); + tree_view_menu->addAction(act); + + // stereo item view menu + this->stereo_item_menu = new QMenu(this); + + act = new QAction("Calibre Stereo Camera", this); + this->stereo_item_menu->addAction(act); + connect(act, SIGNAL(triggered()), this, SLOT(calibreStereoItem())); + this->stereo_item_menu->addAction(act); + + act = new QAction("Save Stereo Camera Parameters", this); + this->stereo_item_menu->addAction(act); + connect(act, SIGNAL(triggered()), this, SLOT(saveStereoItemParameters())); + this->stereo_item_menu->addAction(act); + + this->stereo_item_menu->addAction(act_remove); + + // stereo camera menu + this->stereo_camera_item_menu = new QMenu(this); + act = new QAction("Load Images", this); + this->stereo_camera_item_menu->addAction(act); + connect(act, SIGNAL(triggered()), this, SLOT(loadStereoImages())); + this->stereo_camera_item_menu->addAction(act); + + // stereo image menu + this->stereo_image_item_menu = new QMenu(this); + this->stereo_image_item_menu->addAction(act_remove); + //graphics view image_view = gui.imageView; // add initial camera - addCamera(); + //addCamera(); // progress dialog - progress_dialog_images = new QProgressDialog("loading images","cancel",0,0,this); + progress_dialog_images = new QProgressDialog("loading images", "cancel", 0, 0, this); future_watcher_images = new QFutureWatcher(this); connect(future_watcher_images, SIGNAL(progressValueChanged(int)), progress_dialog_images, SLOT(setValue(int))); connect(future_watcher_images, SIGNAL(progressRangeChanged(int, int)), progress_dialog_images, SLOT(setRange(int, int))); connect(future_watcher_images, SIGNAL(finished()), progress_dialog_images, SLOT(accept())); - progress_dialog_chessboard = new QProgressDialog("searching for chessboards","cancel",0,0,this); - future_watcher_chessboard= new QFutureWatcher >(this); + progress_dialog_chessboard = new QProgressDialog("searching for chessboards", "cancel", 0, 0, this); + future_watcher_chessboard = new QFutureWatcher >(this); connect(future_watcher_chessboard, SIGNAL(progressValueChanged(int)), progress_dialog_chessboard, SLOT(setValue(int))); connect(future_watcher_chessboard, SIGNAL(finished()), progress_dialog_chessboard, SLOT(accept())); - // connect(future_watcher_chessboard, SIGNAL(progressRangeChanged(int, int)), progress_dialog_chessboard, SLOT(setRange(int, int))); + // connect(future_watcher_chessboard, SIGNAL(progressRangeChanged(int, int)), progress_dialog_chessboard, SLOT(setRange(int, int))); - progress_dialog_calibrate = new QProgressDialog("calibrate camera","cancel",0,0,this); + progress_dialog_calibrate = new QProgressDialog("calibrate camera", "cancel", 0, 0, this); progress_dialog_calibrate->setCancelButton(NULL); future_watcher_calibrate = new QFutureWatcher(this); connect(future_watcher_calibrate, SIGNAL(progressValueChanged(int)), progress_dialog_calibrate, SLOT(setValue(int))); connect(future_watcher_calibrate, SIGNAL(finished()), progress_dialog_calibrate, SLOT(accept())); + + // process dialog for stereo procedures + this->progress_dialog_stereo_images_items = new QProgressDialog("Loading images and searching chessboard", "Cancel", 0, 0, this); + this->future_watcher_stereo_images_items = new QFutureWatcher >(this); + connect(this->future_watcher_stereo_images_items, SIGNAL(progressValueChanged(int)), this->progress_dialog_stereo_images_items, SLOT(setValue(int))); + connect(this->future_watcher_stereo_images_items, SIGNAL(progressRangeChanged(int, int)), this->progress_dialog_stereo_images_items, SLOT(setRange(int, int))); + connect(this->future_watcher_stereo_images_items, SIGNAL(finished()), this->progress_dialog_stereo_images_items, SLOT(accept())); + } -QCamCalib::~QCamCalib() -{ +QCamCalib::~QCamCalib() { } -void QCamCalib::contextMenuTreeView(const QPoint &point) -{ +void QCamCalib::contextMenuTreeView(const QPoint &point) { QTreeView *tree_view = findChild("treeView"); - if(!tree_view) + if (!tree_view) throw std::runtime_error("Cannot find treeView object"); QModelIndex index = tree_view->indexAt(point); - if(index.isValid()) - { + if (index.isValid()) { QCamCalibItem *item = dynamic_cast(tree_model->itemFromIndex(index)); - if(!item) + if (!item) return; - else if(dynamic_cast(item)) + else if (dynamic_cast(item)) camera_item_menu->exec(tree_view->viewport()->mapToGlobal(point)); - else if(dynamic_cast(item)) + else if (dynamic_cast(item)) image_item_menu->exec(tree_view->viewport()->mapToGlobal(point)); - } - else { + else if (dynamic_cast(item)) + this->stereo_item_menu->exec(tree_view->viewport()->mapToGlobal(point)); + else if (dynamic_cast(item)) + this->stereo_camera_item_menu->exec(tree_view->viewport()->mapToGlobal(point)); + else if (dynamic_cast(item)) + this->stereo_image_item_menu->exec(tree_view->viewport()->mapToGlobal(point)); + } else { tree_view_menu->exec(tree_view->viewport()->mapToGlobal(point)); } } -void QCamCalib::addCamera(int camera_id) -{ - if(camera_id < 0) +void QCamCalib::addCamera(int camera_id) { + if (camera_id < 0) camera_id = 0; - for(int i=0;i<=tree_model->rowCount();++i) - { + for (int i = 0; i <= tree_model->rowCount(); ++i) { std::stringstream strstr; - strstr << CAMERA_BASE_NAME << (camera_id+i); - QModelIndexList list = tree_model->match(tree_model->index(0,0),Qt::DisplayRole,QVariant(strstr.str().c_str()),1,Qt::MatchExactly); - if(list.empty()) - { - tree_model->appendRow(new qcam_calib::CameraItem(camera_id+i,QString(strstr.str().c_str()))); + strstr << CAMERA_BASE_NAME << (camera_id + i); + QModelIndexList list = tree_model->match(tree_model->index(0, 0), Qt::DisplayRole, QVariant(strstr.str().c_str()), 1, Qt::MatchExactly); + if (list.empty()) { + tree_model->appendRow(new qcam_calib::CameraItem(camera_id + i, QString(strstr.str().c_str()))); break; } } } -void QCamCalib::saveCameraParameter(int camera_id) -{ +void QCamCalib::saveCameraParameter(int camera_id) { CameraItem *item = getCameraItem(camera_id); - if(!item->isCalibrated()) - { + if (!item->isCalibrated()) { calibrateCamera(camera_id); - if(!item->isCalibrated()) + if (!item->isCalibrated()) return; } - QString path = QFileDialog::getSaveFileName(this, "Save Parameter",current_load_path, "config (*.yml *.xml)"); - if(path.size() != 0) + QString path = QFileDialog::getSaveFileName(this, "Save Parameter", current_load_path, "config (*.yml *.xml)"); + if (path.size() != 0) item->saveParameter(path); } -void QCamCalib::removeCamera(int camera_id) -{ +void QCamCalib::removeCamera(int camera_id) { CameraItem *item = getCameraItem(camera_id); tree_model->removeRow(item->row()); } -QImage loadImage(const QString &path) -{ +QImage loadImage(const QString &path) { return QImage(path); } -void QCamCalib::loadImages(int camera_id) -{ +void QCamCalib::loadImages(int camera_id) { QSpinBox *cols = findChild("spinBoxCols"); QSpinBox *rows = findChild("spinBoxRows"); - if(!cols || !rows) + if (!cols || !rows) throw std::runtime_error("cannot find chessboard config"); //select images - QStringList paths = QFileDialog::getOpenFileNames(this, "Open images",current_load_path, "Images (*.png *.jpg)"); + QStringList paths = QFileDialog::getOpenFileNames(this, "Open images", current_load_path, "Images (*.png *.jpg)"); //load images in parallel - QFuture images = QtConcurrent::mapped(paths,loadImage); + QFuture images = QtConcurrent::mapped(paths, loadImage); future_watcher_images->setFuture(images); - if(QDialog::Accepted != progress_dialog_images->exec() && future_watcher_images->isCanceled()) + if (QDialog::Accepted != progress_dialog_images->exec() && future_watcher_images->isCanceled()) return; future_watcher_images->waitForFinished(); //progress_dialog_images->close(); //find chess boards in parallel QFuture > chessboards; - chessboards = QtConcurrent::mapped(images, - boost::bind(static_cast(*)(const QImage&,int,int)>(ImageItem::findChessboard),_1,cols->value(),rows->value())); + chessboards = QtConcurrent::mapped(images, boost::bind(static_cast (*)(const QImage&, int, int)>(ImageItem::findChessboard), _1,cols->value(),rows->value())); future_watcher_chessboard->setFuture(chessboards); - progress_dialog_chessboard->setRange(progress_dialog_images->minimum(),progress_dialog_images->maximum()); - if(QDialog::Accepted != progress_dialog_chessboard->exec() && future_watcher_chessboard->isCanceled()) + progress_dialog_chessboard->setRange(progress_dialog_images->minimum(), progress_dialog_images->maximum()); + if (QDialog::Accepted != progress_dialog_chessboard->exec() && future_watcher_chessboard->isCanceled()) return; future_watcher_chessboard->waitForFinished(); progress_dialog_chessboard->close(); @@ -200,158 +224,249 @@ void QCamCalib::loadImages(int camera_id) QFuture::const_iterator iter_image = images.begin(); QFuture >::const_iterator iter_chessboard = chessboards.begin(); ImageItem *last_image_item = NULL; - for(;iter_path!=paths.end() && iter_image!=images.end() && iter_chessboard!=chessboards.end();++iter_path,++iter_image,++iter_chessboard) - { + for (; iter_path != paths.end() && iter_image != images.end() && iter_chessboard != chessboards.end(); ++iter_path, ++iter_image, ++iter_chessboard) { QFileInfo info(*iter_path); current_load_path = info.absolutePath(); CameraItem *item = getCameraItem(camera_id); - last_image_item = item->addImage(info.fileName(),*iter_image); - last_image_item->setChessboard(*iter_chessboard,cols->value(),rows->value()); + last_image_item = item->addImage(info.fileName(), *iter_image); + last_image_item->setChessboard(*iter_chessboard, cols->value(), rows->value()); } - if(last_image_item) + if (last_image_item) displayImage(last_image_item->getImage()); } -void QCamCalib::calibrateCamera(int camera_id) -{ +void QCamCalib::calibrateCamera(int camera_id) { QSpinBox *cols = findChild("spinBoxCols"); QSpinBox *rows = findChild("spinBoxRows"); QDoubleSpinBox *dx = findChild("spinBoxDx"); QDoubleSpinBox *dy = findChild("spinBoxDy"); - if(!cols || !rows || !dx || !dy) + if (!cols || !rows || !dx || !dy) throw std::runtime_error("cannot find chessboard config"); //select images CameraItem *item = getCameraItem(camera_id); - if(item->countChessboards() < 5) - { + if (item->countChessboards() < 5) { QErrorMessage box; - box.showMessage("Not enough chessboards for calibration. Add more images." ); + box.showMessage("Not enough chessboards for calibration. Add more images."); box.exec(); return; } - future_watcher_calibrate->setFuture(QtConcurrent::run(item,&CameraItem::calibrate,cols->value(),rows->value(),dx->value(),dy->value())); - progress_dialog_calibrate->setRange(0,0); - if(QDialog::Accepted != progress_dialog_calibrate->exec() && future_watcher_calibrate->isCanceled()) + future_watcher_calibrate->setFuture(QtConcurrent::run(item, &CameraItem::calibrate, cols->value(), rows->value(), dx->value(), dy->value())); + progress_dialog_calibrate->setRange(0, 0); + if (QDialog::Accepted != progress_dialog_calibrate->exec() && future_watcher_calibrate->isCanceled()) return; } -CameraItem *QCamCalib::getCameraItem(int camera_id) -{ +CameraItem *QCamCalib::getCameraItem(int camera_id) { CameraItem *item = NULL; - if(camera_id < 0) - { + if (camera_id < 0) { QTreeView *tree_view = findChild("treeView"); - if(!tree_view) + if (!tree_view) throw std::runtime_error("Cannot find treeView object"); QModelIndex index = tree_view->currentIndex(); - if(index.isValid()) + if (index.isValid()) item = dynamic_cast(tree_model->itemFromIndex(index)); - } - else - { - for(int i=0;irowCount();++i) - { - item = dynamic_cast(tree_model->item(i,0)); - if(item && item->getId() == camera_id) + } else { + for (int i = 0; i < tree_model->rowCount(); ++i) { + item = dynamic_cast(tree_model->item(i, 0)); + if (item && item->getId() == camera_id) break; else item = NULL; } } - if(item == NULL) + if (item == NULL) throw std::runtime_error("Internal error: cannot find camera"); return item; } -ImageItem *QCamCalib::getImageItem(int camera_id,const QString &name) -{ +ImageItem *QCamCalib::getImageItem(int camera_id, const QString &name) { ImageItem *item = NULL; - if(camera_id < 0) - { + if (camera_id < 0) { QTreeView *tree_view = findChild("treeView"); - if(!tree_view) + if (!tree_view) throw std::runtime_error("Cannot find treeView object"); QModelIndex index = tree_view->currentIndex(); - if(index.isValid()) + if (index.isValid()) item = dynamic_cast(tree_model->itemFromIndex(index)); - } - else - { - for(int i=0;irowCount()&&!item;++i) - { - CameraItem *camera = dynamic_cast(tree_model->item(i,0)); - if(camera) + } else { + for (int i = 0; i < tree_model->rowCount() && !item; ++i) { + CameraItem *camera = dynamic_cast(tree_model->item(i, 0)); + if (camera) item = camera->getImageItem(name); } } - if(!item) + if (!item) throw std::runtime_error("Internal error: cannot find image item"); return item; } -void QCamCalib::addImage(const QString &name,const QImage &image,int camera_id) -{ +void QCamCalib::addImage(const QString &name, const QImage &image, int camera_id) { CameraItem *item = getCameraItem(camera_id); - item->addImage(name,image); + item->addImage(name, image); } -void QCamCalib::findChessBoard(int camera_id,const QString &name) -{ +void QCamCalib::findChessBoard(int camera_id, const QString &name) { QSpinBox *cols = findChild("spinBoxCols"); QSpinBox *rows = findChild("spinBoxRows"); - if(!cols || !rows) + if (!cols || !rows) throw std::runtime_error("cannot find chessboard config"); - ImageItem *item = getImageItem(camera_id,name); + ImageItem *item = getImageItem(camera_id, name); QList images; images.push_back(item->getRawImage()); QFuture > chessboards; - chessboards = QtConcurrent::mapped(images, - boost::bind(static_cast(*)(const QImage&,int,int)>(ImageItem::findChessboard),_1,cols->value(),rows->value())); + chessboards = QtConcurrent::mapped(images, boost::bind(static_cast (*)(const QImage&, int, int)>(ImageItem::findChessboard), _1,cols->value(),rows->value())); future_watcher_chessboard->setFuture(chessboards); - progress_dialog_chessboard->setRange(0,1); - if(QDialog::Accepted != progress_dialog_chessboard->exec() && future_watcher_chessboard->isCanceled()) + progress_dialog_chessboard->setRange(0, 1); + if (QDialog::Accepted != progress_dialog_chessboard->exec() && future_watcher_chessboard->isCanceled()) return; - item->setChessboard(chessboards.results().front(),cols->value(),rows->value()); + item->setChessboard(chessboards.results().front(), cols->value(), rows->value()); displayImage(item->getImage()); } -void QCamCalib::removeCurrentItem() -{ +void QCamCalib::removeCurrentItem() { QTreeView *tree_view = findChild("treeView"); - if(!tree_view) + if (!tree_view) throw std::runtime_error("Cannot find treeView object"); QModelIndex index = tree_view->currentIndex(); - if(index.isValid()) - { - if(index.parent().isValid()) + if (index.isValid()) { + if (index.parent().isValid()) tree_model->itemFromIndex(index.parent())->removeRow(index.row()); else tree_model->removeRow(index.row()); } } -void QCamCalib::displayImage(const QImage &image) -{ +void QCamCalib::displayImage(const QImage &image) { image_view->displayImage(image); } -void QCamCalib::clickedTreeView(const QModelIndex& index) -{ - if(!index.isValid()) +void QCamCalib::clickedTreeView(const QModelIndex& index) { + if (!index.isValid()) return; const QStandardItemModel *model = dynamic_cast(index.model()); - if(!model) + if (!model) return; QStandardItem *item = model->itemFromIndex(index); - if(!item) + if (!item) return; ImageItem *image = dynamic_cast(item); - if(image) + if (image) image_view->displayImage(image->getImage()); + + StereoImageItem *stereo_image = dynamic_cast(item); + QSpinBox *cols = findChild("spinBoxCols"); + QSpinBox *rows = findChild("spinBoxRows"); + if (stereo_image) + image_view->displayImage(stereo_image->getImageWithChessboard(cols->value(), rows->value())); +} + +void QCamCalib::addStereoItem(int camera_id) { + if (camera_id < 0) + camera_id = 0; + for (int i = 0; i <= tree_model->rowCount(); ++i) { + std::stringstream strstr; + strstr << qcam_calib::StereoItem::getBaseName() << (camera_id + i); + QModelIndexList list = tree_model->match(tree_model->index(0, 0), Qt::DisplayRole, QVariant(strstr.str().c_str()), 1, Qt::MatchExactly); + if (list.empty()) { + tree_model->appendRow(new qcam_calib::StereoItem(camera_id + i, QString(strstr.str().c_str()))); + break; + } + } +} + +template +T QCamCalib::getItems(int item_id, QString erro_msg) { + T item = NULL; + if (item_id < 0) { + QTreeView *tree_view = findChild("treeView"); + if (!tree_view) + throw std::runtime_error("Cannot find treeView object"); + QModelIndex index = tree_view->currentIndex(); + if (index.isValid()) + item = dynamic_cast(tree_model->itemFromIndex(index)); + } else { + for (int i = 0; i < tree_model->rowCount(); ++i) { + item = dynamic_cast(tree_model->item(i, 0)); + if (item && item->getId() == item_id) + break; + else + item = NULL; + } + } + if (item == NULL) + throw std::runtime_error(erro_msg.toStdString()); + return item; } +void QCamCalib::loadStereoImages(int camera_id) { + + QSpinBox *cols = findChild("spinBoxCols"); + QSpinBox *rows = findChild("spinBoxRows"); + if (!cols || !rows) + throw std::runtime_error("Cannot find chessboard config!!"); + //select images + QStringList paths = QFileDialog::getOpenFileNames(this, "Open images", current_load_path, "Images (*.png *.jpg)"); + + //load stereo image items in parallel + QFuture > stereoImagesItems = QtConcurrent::mapped(paths, + boost::bind(static_cast (*)(const QString&, int, int)>(StereoTools::loadStereoImageAndFindChessboardItem), _1,cols->value(),rows->value())); + + //progress bar + this->future_watcher_stereo_images_items->setFuture(stereoImagesItems); + if (QDialog::Accepted != this->progress_dialog_stereo_images_items->exec() && this->future_watcher_stereo_images_items->isCanceled()) + return; + this->future_watcher_stereo_images_items->waitForFinished(); + + //add images items to camera + QString error_msg = "Internal error: Cannot find camera"; + StereoCameraItem *item = getItems(camera_id, error_msg); + + QFuture >::const_iterator iterStereoImageitems; + for (iterStereoImageitems = stereoImagesItems.begin(); iterStereoImageitems != stereoImagesItems.end(); ++iterStereoImageitems) + item->addImages(*iterStereoImageitems); +} + + +void QCamCalib::saveStereoItemParameters(int stereo_id) { + QString erro_msg = "Internal error: Cannot find stereo camera."; + StereoItem *item = getItems(stereo_id, erro_msg); + + if (!item->isCalibrated()) { + calibreStereoItem(stereo_id); + if (!item->isCalibrated()) + return; + } + QString path = QFileDialog::getSaveFileName(this, "Save Parameter", current_load_path, "config (*.yml *.xml)"); + if (path.size() != 0) + item->saveParameter(path); +} + +void QCamCalib::calibreStereoItem(int stereo_id) { + QSpinBox *cols = findChild("spinBoxCols"); + QSpinBox *rows = findChild("spinBoxRows"); + QDoubleSpinBox *dx = findChild("spinBoxDx"); + QDoubleSpinBox *dy = findChild("spinBoxDy"); + if (!cols || !rows || !dx || !dy) + throw std::runtime_error("Cannot find chessboard config."); + + QString erro_msg = "Internal error: Cannot find stereo camera."; + StereoItem* stereo_item = getItems(stereo_id, erro_msg); + + try { + future_watcher_calibrate->setFuture(QtConcurrent::run(stereo_item, &StereoItem::calibrate, cols->value(), rows->value(), dx->value(), dy->value())); + progress_dialog_calibrate->setRange(0, 0); + if (QDialog::Accepted != progress_dialog_calibrate->exec() && future_watcher_calibrate->isCanceled()) + return; + } catch (std::exception& e) { + QErrorMessage box; + box.showMessage(QString(e.what())); + box.exec(); + return; + } + +} diff --git a/src/QCamCalib.hpp b/src/QCamCalib.hpp index 1f203aa..6dea232 100644 --- a/src/QCamCalib.hpp +++ b/src/QCamCalib.hpp @@ -5,14 +5,13 @@ #include #include -namespace qcam_calib -{ - class CameraItem; - class ImageItem; - class ImageView; +namespace qcam_calib { +class CameraItem; +class ImageItem; +class ImageView; +class StereoCameraItem; } - /** * \brief Widget for calibrating pinhole cameras * @@ -22,8 +21,7 @@ namespace qcam_calib * * \author Alexander.Duda@dfki.de */ -class QCamCalib : public QWidget -{ +class QCamCalib: public QWidget { Q_OBJECT public: QCamCalib(QWidget *parent = 0); @@ -70,7 +68,7 @@ public slots: * \param[in] camera_id The id of the camera. * \author Alexander.Duda@dfki.de */ - void addImage(const QString &name,const QImage &image,int camera_id=-1); + void addImage(const QString &name, const QImage &image, int camera_id = -1); /** * \brief Opens a file dialog and saves the camera parameter as YAML or XML @@ -101,7 +99,23 @@ public slots: * \param[in] name The name of the image * \author Alexander.Duda@dfki.de */ - void findChessBoard(int camera_id=-1,const QString &name=QString("")); + void findChessBoard(int camera_id = -1, const QString &name = QString("")); + + + // stereo camrea methods + /** + * \brief Adds a new stereo camera to the workspace + * + * \param[in] camera_id The id of the stereo camera. If no id is given the next free id is used starting from 0 + * \author trocolit2@gmail.com + */ + void addStereoItem(int camera_id = -1); + + void saveStereoItemParameters(int camera_id = -1); + + void calibreStereoItem(int stereo_id = -1); + + void loadStereoImages(int camera_id = -1); private slots: void contextMenuTreeView(const QPoint &point); @@ -111,7 +125,14 @@ private slots: private: qcam_calib::CameraItem *getCameraItem(int camera_id); - qcam_calib::ImageItem *getImageItem(int camera_id,const QString &name); + qcam_calib::ImageItem *getImageItem(int camera_id, const QString &name); + + //find camera from stereo item + template + T getItems(int item_id, QString erro_msg); + + + private: // file paths @@ -125,6 +146,10 @@ private slots: QMenu *tree_view_menu; QMenu *image_item_menu; + QMenu *stereo_item_menu; + QMenu *stereo_camera_item_menu; + QMenu *stereo_image_item_menu; + // image dispay qcam_calib::ImageView *image_view; @@ -135,6 +160,9 @@ private slots: QFutureWatcher *future_watcher_images; QFutureWatcher > *future_watcher_chessboard; QFutureWatcher *future_watcher_calibrate; + + QProgressDialog *progress_dialog_stereo_images_items; + QFutureWatcher > *future_watcher_stereo_images_items; }; #endif /* QCAMCALIB_HPP */ diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt new file mode 100644 index 0000000..eb7061a --- /dev/null +++ b/test/CMakeLists.txt @@ -0,0 +1,5 @@ +rock_testsuite(stereoTools_test StereoTools_test.cpp + DEPS QCamCalib + LIBS ${Boost_THREAD_LIBRARY} ${Boost_SYSTEM_LIBRARY} + DEPS_PKGCONFIG opencv) + diff --git a/test/StereoTools_test.cpp b/test/StereoTools_test.cpp new file mode 100644 index 0000000..f11ebcc --- /dev/null +++ b/test/StereoTools_test.cpp @@ -0,0 +1,183 @@ +/* + * StereoTools_test.cpp + * + * Created on: Jan 18, 2016 + * Author: tiagotrocoli + */ + +#define BOOST_TEST_MODULE "StereoTools_test" +#include + +#include +#include +#include + +#include +#include + +#include + +#include + +#define IMAGE_STEREO_CHESSBOARD "../../test/resource/stereo_dataset" +#define NO_CHESSBOARD "No Chessboard" +#define OK_CHESSBOARD "OK, Chessboard Found" + +using namespace qcam_calib; + +int precision = 1000; +int chessboard_cols = 9; +int chessboard_rows = 6; +int chessboard_dx = 35; +int chessboard_dy = 35; +float calibrate_error_rms = 0.5659; +float calibrate_avarege_error = 4.8452; + +int ground_truth_points[][54 * 2] = { { 244, 94, 274, 92, 305, 90, 338, 88, 371, 87, 406, 86, 441, 86, 477, 86, 514, 86, 244, 126, 275, 125, 306, 124, 338, 123, 372, 122, 406, 122, 442, 122, 478, 122, + 513, 123, 245, 158, 275, 158, 306, 157, 338, 157, 372, 157, 406, 157, 441, 157, 477, 158, 513, 158, 246, 190, 275, 190, 307, 190, 339, 191, 372, 191, 406, 192, 441, 193, 477, 194, 513, 195, + 247, 222, 276, 223, 307, 224, 339, 225, 372, 226, 406, 227, 440, 228, 476, 229, 511, 231, 248, 253, 277, 255, 308, 256, 340, 258, 372, 259, 406, 261, 440, 262, 475, 264, 510, 266 }, + +{ 256, 357, 255, 334, 254, 308, 253, 280, 252, 248, 252, 213, 251, 172, 251, 128, 251, 77, 291, 366, 292, 343, 293, 318, 294, 290, 296, 257, 298, 221, 301, 182, 304, 135, 306, 86, 327, 374, 331, 352, + 334, 327, 337, 299, 342, 267, 347, 233, 352, 192, 357, 145, 365, 96, 367, 382, 369, 360, 375, 335, 381, 308, 388, 277, 396, 241, 404, 202, 413, 158, 423, 106, 401, 389, 407, 368, 416, 344, + 425, 317, 433, 287, 445, 252, 456, 213, 469, 169, 482, 120, 437, 396, 446, 376, 456, 352, 467, 326, 480, 296, 492, 262, 508, 224, 523, 181, 539, 131 }, + +{ 277, 72, 313, 81, 352, 91, 394, 101, 434, 113, 476, 126, 519, 140, 561, 154, 604, 169, 259, 106, 297, 114, 336, 126, 378, 137, 421, 150, 465, 165, 508, 178, 552, 193, 594, 208, 242, 141, 279, 152, + 320, 164, 364, 177, 406, 190, 450, 205, 495, 219, 540, 235, 585, 250, 224, 177, 262, 191, 303, 204, 345, 218, 389, 233, 435, 248, 481, 264, 528, 280, 573, 295, 205, 217, 244, 230, 284, 245, + 328, 261, 372, 276, 419, 293, 465, 309, 513, 326, 560, 342, 187, 257, 225, 272, 267, 289, 309, 305, 354, 323, 402, 340, 449, 358, 496, 373, 545, 390 } }; + +std::string intToStr(int a) { + std::stringstream ss; + ss << a; + return ss.str(); +} + +BOOST_AUTO_TEST_CASE(convertOpenCVpointstoQt_testcase) { + + std::vector pointsCV; + pointsCV.push_back(cv::Point2f(0, 0)); + pointsCV.push_back(cv::Point2f(-0.1235, 0.1235)); + pointsCV.push_back(cv::Point2f(1.2345, 0.1235)); + pointsCV.push_back(cv::Point2f(0.2345, -1.1235)); + + QVector qtPoints = StereoTools::convertVectorPoints2fToQVectorQPointF(pointsCV); + + QVector::const_iterator iterQt = qtPoints.begin(); + std::vector::const_iterator iterCV = pointsCV.begin(); + for (; iterCV != pointsCV.end(); ++iterCV, ++iterQt) { + BOOST_CHECK_EQUAL((int )(iterQt->x() * precision), (int ) (iterCV->x * precision)); + BOOST_CHECK_EQUAL((int )(iterQt->y() * precision), (int )(iterCV->y * precision)); + } +} + +BOOST_AUTO_TEST_CASE(convertQtpointstoOpenCV_testcase) { + + QVector qt_points; + qt_points.push_back(QPointF(0, 0)); + qt_points.push_back(QPointF(-0.123, 0.123)); + qt_points.push_back(QPointF(1.234, 0.123)); + qt_points.push_back(QPointF(0.234, -1.123)); + + std::vector cv_points = StereoTools::convertQVectorQPointFToVectorPoints2f(qt_points); + + QVector::const_iterator iterQt = qt_points.begin(); + std::vector::const_iterator iterCV = cv_points.begin(); + for (; iterQt != qt_points.end(); ++iterCV, ++iterQt) { + BOOST_CHECK_EQUAL((int )(iterQt->x() * precision), (int ) (iterCV->x * precision)); + BOOST_CHECK_EQUAL((int )(iterQt->y() * precision), (int )(iterCV->y * precision)); + } +} + +BOOST_AUTO_TEST_CASE(testSearchChessboard_testcase) { + + std::string root_path = std::string(IMAGE_STEREO_CHESSBOARD) + "/left_images/left"; + for (int i = 1; i <= 3; ++i) { + + std::string temp_path = root_path + "0" + intToStr(i) + ".jpg"; + QVector qt_points = StereoTools::findChessboard(QString::fromUtf8(temp_path.c_str()), chessboard_cols, chessboard_rows); + + int count_points_gt = 0; + QVector::const_iterator iterQt = qt_points.begin(); + for (; iterQt != qt_points.end(); ++iterQt) { + BOOST_CHECK_EQUAL((int )iterQt->x(), ground_truth_points[i - 1][count_points_gt++]); + BOOST_CHECK_EQUAL((int )iterQt->y(), ground_truth_points[i - 1][count_points_gt++]); + } + } +} + +BOOST_AUTO_TEST_CASE(loadStereoImageAndFindChessboardItem_test) { + + std::string root_path = std::string(IMAGE_STEREO_CHESSBOARD) + "/left_images/left"; + for (int i = 1; i <= 3; ++i) { + + std::string temp_path = root_path + "0" + intToStr(i) + ".jpg"; + QList items = StereoTools::loadStereoImageAndFindChessboardItem(QString::fromUtf8(temp_path.c_str()), chessboard_cols, chessboard_rows); + StereoImageItem *item = dynamic_cast(items.at(0)); + + BOOST_CHECK_EQUAL(items.at(1)->text().toStdString(), std::string(OK_CHESSBOARD)); + + int count_points_gt = 0; + QVector::const_iterator iterQt = item->getChessboardCorners().begin(); + for (; iterQt != item->getChessboardCorners().end(); ++iterQt) { + BOOST_CHECK_EQUAL((int )iterQt->x(), ground_truth_points[i - 1][count_points_gt++]); + BOOST_CHECK_EQUAL((int )iterQt->y(), ground_truth_points[i - 1][count_points_gt++]); + } + } +} + +BOOST_AUTO_TEST_CASE(stereoCalibrate_test) { + + std::string root_path_left = std::string(IMAGE_STEREO_CHESSBOARD) + "/left_images/left"; + std::string root_path_right = std::string(IMAGE_STEREO_CHESSBOARD) + "/right_images/right"; + std::vector > left_points, right_points; + std::vector > object_points; + + std::vector chessboard_3DPonts; + for (int row = 0; row < chessboard_rows; ++row) + for (int col = 0; col < chessboard_cols; ++col) + chessboard_3DPonts.push_back(cv::Point3f(chessboard_dx * col, chessboard_dy * row, 0)); + + int image_sample = -1; + for (int i = 1; i <= 14; ++i) { + + std::string temp_path_left = root_path_left; + std::string temp_path_right = root_path_right; + + if (i < 10) { + temp_path_left += "0"; + temp_path_right += "0"; + } + + temp_path_left += intToStr(i) + ".jpg"; + temp_path_right += intToStr(i) + ".jpg"; + + QList items_left = StereoTools::loadStereoImageAndFindChessboardItem(QString::fromUtf8(temp_path_left.c_str()), chessboard_cols, chessboard_rows); + StereoImageItem *item_left = dynamic_cast(items_left.at(0)); + + QList items_right = StereoTools::loadStereoImageAndFindChessboardItem(QString::fromUtf8(temp_path_right.c_str()), chessboard_cols, chessboard_rows); + StereoImageItem *item_right = dynamic_cast(items_right.at(0)); + + if (!(item_right->isChessboardFound() && item_left->isChessboardFound())) + continue; + + left_points.push_back(StereoTools::convertQVectorQPointFToVectorPoints2f(item_left->getChessboardCorners())); + right_points.push_back(StereoTools::convertQVectorQPointFToVectorPoints2f(item_right->getChessboardCorners())); + object_points.push_back(chessboard_3DPonts); + image_sample = i; + } + + std::string temp_path = root_path_left + intToStr(image_sample) + ".jpg"; + QImage qt_image(QString::fromUtf8(temp_path.c_str())); + qt_image = qt_image.convertToFormat(QImage::Format_RGB888); + cv::Size image_size(qt_image.height(), qt_image.width()); + + cv::Mat camera_matrix[2], dist_coeffs[2]; + camera_matrix[0] = cv::Mat::eye(3, 3, CV_64F); + camera_matrix[1] = cv::Mat::eye(3, 3, CV_64F); + cv::Mat rotation, translation, essential, fundamental; + + std::vector vec_matrix = StereoTools::stereoCalibrate(left_points, right_points, object_points, image_size); + + BOOST_CHECK_EQUAL((int )(calibrate_error_rms * precision), (int )(vec_matrix[4].at(0) * precision)); + BOOST_CHECK_EQUAL((int )(calibrate_avarege_error * precision), (int )(vec_matrix[4].at(1) * precision)); + +} diff --git a/test/resource/stereo_dataset/left_images/left01.jpg b/test/resource/stereo_dataset/left_images/left01.jpg new file mode 100644 index 0000000..6ade320 Binary files /dev/null and b/test/resource/stereo_dataset/left_images/left01.jpg differ diff --git a/test/resource/stereo_dataset/left_images/left02.jpg b/test/resource/stereo_dataset/left_images/left02.jpg new file mode 100644 index 0000000..457820e Binary files /dev/null and b/test/resource/stereo_dataset/left_images/left02.jpg differ diff --git a/test/resource/stereo_dataset/left_images/left03.jpg b/test/resource/stereo_dataset/left_images/left03.jpg new file mode 100644 index 0000000..7f3990f Binary files /dev/null and b/test/resource/stereo_dataset/left_images/left03.jpg differ diff --git a/test/resource/stereo_dataset/left_images/left04.jpg b/test/resource/stereo_dataset/left_images/left04.jpg new file mode 100644 index 0000000..3144a18 Binary files /dev/null and b/test/resource/stereo_dataset/left_images/left04.jpg differ diff --git a/test/resource/stereo_dataset/left_images/left05.jpg b/test/resource/stereo_dataset/left_images/left05.jpg new file mode 100644 index 0000000..7b57131 Binary files /dev/null and b/test/resource/stereo_dataset/left_images/left05.jpg differ diff --git a/test/resource/stereo_dataset/left_images/left06.jpg b/test/resource/stereo_dataset/left_images/left06.jpg new file mode 100644 index 0000000..1c6a217 Binary files /dev/null and b/test/resource/stereo_dataset/left_images/left06.jpg differ diff --git a/test/resource/stereo_dataset/left_images/left07.jpg b/test/resource/stereo_dataset/left_images/left07.jpg new file mode 100644 index 0000000..46acbb9 Binary files /dev/null and b/test/resource/stereo_dataset/left_images/left07.jpg differ diff --git a/test/resource/stereo_dataset/left_images/left08.jpg b/test/resource/stereo_dataset/left_images/left08.jpg new file mode 100644 index 0000000..0fe7646 Binary files /dev/null and b/test/resource/stereo_dataset/left_images/left08.jpg differ diff --git a/test/resource/stereo_dataset/left_images/left09.jpg b/test/resource/stereo_dataset/left_images/left09.jpg new file mode 100644 index 0000000..5d17e95 Binary files /dev/null and b/test/resource/stereo_dataset/left_images/left09.jpg differ diff --git a/test/resource/stereo_dataset/left_images/left10.jpg b/test/resource/stereo_dataset/left_images/left10.jpg new file mode 100644 index 0000000..2755192 Binary files /dev/null and b/test/resource/stereo_dataset/left_images/left10.jpg differ diff --git a/test/resource/stereo_dataset/left_images/left11.jpg b/test/resource/stereo_dataset/left_images/left11.jpg new file mode 100644 index 0000000..7ca2a14 Binary files /dev/null and b/test/resource/stereo_dataset/left_images/left11.jpg differ diff --git a/test/resource/stereo_dataset/left_images/left12.jpg b/test/resource/stereo_dataset/left_images/left12.jpg new file mode 100644 index 0000000..897955d Binary files /dev/null and b/test/resource/stereo_dataset/left_images/left12.jpg differ diff --git a/test/resource/stereo_dataset/left_images/left13.jpg b/test/resource/stereo_dataset/left_images/left13.jpg new file mode 100644 index 0000000..e2f060f Binary files /dev/null and b/test/resource/stereo_dataset/left_images/left13.jpg differ diff --git a/test/resource/stereo_dataset/left_images/left14.jpg b/test/resource/stereo_dataset/left_images/left14.jpg new file mode 100644 index 0000000..1dec7c6 Binary files /dev/null and b/test/resource/stereo_dataset/left_images/left14.jpg differ diff --git a/test/resource/stereo_dataset/left_images/test b/test/resource/stereo_dataset/left_images/test new file mode 100644 index 0000000..cf33740 --- /dev/null +++ b/test/resource/stereo_dataset/left_images/test @@ -0,0 +1,14 @@ +%YAML:1.0 +calibrationDate: "Fri Jan 15 10:34:50 2016\n" +cameraMatrix: !!opencv-matrix + rows: 3 + cols: 3 + dt: d + data: [ 5.3073900000000003e+02, 0., 3.4220600000000002e+02, 0., + 5.3150800000000004e+02, 2.3821299999999999e+02, 0., 0., 1. ] +distCoeffs: !!opencv-matrix + rows: 4 + cols: 1 + dt: d + data: [ -2.6834100000000000e-01, -7.9057000000000002e-02, + 1.1427500000000001e-03, -4.0985899999999999e-04 ] diff --git a/test/resource/stereo_dataset/right_images/right01.jpg b/test/resource/stereo_dataset/right_images/right01.jpg new file mode 100644 index 0000000..4d6071b Binary files /dev/null and b/test/resource/stereo_dataset/right_images/right01.jpg differ diff --git a/test/resource/stereo_dataset/right_images/right02.jpg b/test/resource/stereo_dataset/right_images/right02.jpg new file mode 100644 index 0000000..6c0fab0 Binary files /dev/null and b/test/resource/stereo_dataset/right_images/right02.jpg differ diff --git a/test/resource/stereo_dataset/right_images/right03.jpg b/test/resource/stereo_dataset/right_images/right03.jpg new file mode 100644 index 0000000..7502761 Binary files /dev/null and b/test/resource/stereo_dataset/right_images/right03.jpg differ diff --git a/test/resource/stereo_dataset/right_images/right04.jpg b/test/resource/stereo_dataset/right_images/right04.jpg new file mode 100644 index 0000000..958973b Binary files /dev/null and b/test/resource/stereo_dataset/right_images/right04.jpg differ diff --git a/test/resource/stereo_dataset/right_images/right05.jpg b/test/resource/stereo_dataset/right_images/right05.jpg new file mode 100644 index 0000000..926c825 Binary files /dev/null and b/test/resource/stereo_dataset/right_images/right05.jpg differ diff --git a/test/resource/stereo_dataset/right_images/right06.jpg b/test/resource/stereo_dataset/right_images/right06.jpg new file mode 100644 index 0000000..69a7994 Binary files /dev/null and b/test/resource/stereo_dataset/right_images/right06.jpg differ diff --git a/test/resource/stereo_dataset/right_images/right07.jpg b/test/resource/stereo_dataset/right_images/right07.jpg new file mode 100644 index 0000000..9aff92b Binary files /dev/null and b/test/resource/stereo_dataset/right_images/right07.jpg differ diff --git a/test/resource/stereo_dataset/right_images/right08.jpg b/test/resource/stereo_dataset/right_images/right08.jpg new file mode 100644 index 0000000..abb794a Binary files /dev/null and b/test/resource/stereo_dataset/right_images/right08.jpg differ diff --git a/test/resource/stereo_dataset/right_images/right09.jpg b/test/resource/stereo_dataset/right_images/right09.jpg new file mode 100644 index 0000000..a6dd6f9 Binary files /dev/null and b/test/resource/stereo_dataset/right_images/right09.jpg differ diff --git a/test/resource/stereo_dataset/right_images/right10.jpg b/test/resource/stereo_dataset/right_images/right10.jpg new file mode 100644 index 0000000..1551169 Binary files /dev/null and b/test/resource/stereo_dataset/right_images/right10.jpg differ diff --git a/test/resource/stereo_dataset/right_images/right11.jpg b/test/resource/stereo_dataset/right_images/right11.jpg new file mode 100644 index 0000000..29b99be Binary files /dev/null and b/test/resource/stereo_dataset/right_images/right11.jpg differ diff --git a/test/resource/stereo_dataset/right_images/right12.jpg b/test/resource/stereo_dataset/right_images/right12.jpg new file mode 100644 index 0000000..35b2d07 Binary files /dev/null and b/test/resource/stereo_dataset/right_images/right12.jpg differ diff --git a/test/resource/stereo_dataset/right_images/right13.jpg b/test/resource/stereo_dataset/right_images/right13.jpg new file mode 100644 index 0000000..d504a46 Binary files /dev/null and b/test/resource/stereo_dataset/right_images/right13.jpg differ diff --git a/test/resource/stereo_dataset/right_images/right14.jpg b/test/resource/stereo_dataset/right_images/right14.jpg new file mode 100644 index 0000000..2decf1e Binary files /dev/null and b/test/resource/stereo_dataset/right_images/right14.jpg differ diff --git a/test/resource/stereo_dataset/right_images_no_chessboard/right13.jpg b/test/resource/stereo_dataset/right_images_no_chessboard/right13.jpg new file mode 100644 index 0000000..b75dbe7 Binary files /dev/null and b/test/resource/stereo_dataset/right_images_no_chessboard/right13.jpg differ diff --git a/test/resource/stereo_dataset/right_images_no_chessboard/right14.jpg b/test/resource/stereo_dataset/right_images_no_chessboard/right14.jpg new file mode 100644 index 0000000..f21846d Binary files /dev/null and b/test/resource/stereo_dataset/right_images_no_chessboard/right14.jpg differ