diff --git a/CMakeLists.txt b/CMakeLists.txt index f22ef2ab..56487867 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.5.1) # Extract package name and version from package.xml find_package(ros_industrial_cmake_boilerplate REQUIRED) extract_package_metadata(pkg) -project(${pkg_extracted_name} VERSION ${pkg_extracted_version} LANGUAGES CXX) +project(${pkg_extracted_name} VERSION ${pkg_extracted_version} LANGUAGES C CXX) set(SUPPORTED_COMPONENTS "") diff --git a/gui/CMakeLists.txt b/gui/CMakeLists.txt index 332e2444..7f0b9627 100644 --- a/gui/CMakeLists.txt +++ b/gui/CMakeLists.txt @@ -5,6 +5,7 @@ set(CMAKE_AUTOUIC ON) find_package(Qt5 COMPONENTS Widgets REQUIRED) find_package(boost_plugin_loader REQUIRED) +find_package(PCL REQUIRED COMPONENTS common io) # Add widget plugin add_library( @@ -31,22 +32,30 @@ add_library( # Extrinsic hand-eye calibration widget src/extrinsic_hand_eye_calibration_widget.ui src/extrinsic_hand_eye_calibration_widget.cpp + src/extrinsic_hand_eye_calibration_3d_widget.ui + src/extrinsic_hand_eye_calibration_3d_widget.cpp # Camera intrinsic calibration widget src/camera_intrinsic_calibration_widget.ui src/camera_intrinsic_calibration_widget.cpp) target_include_directories(${PROJECT_NAME}_gui PUBLIC "$" - "$") + "$" + ${PCL_INCLUDE_DIRS}) target_link_libraries( ${PROJECT_NAME}_gui PUBLIC Qt5::Widgets boost_plugin_loader::boost_plugin_loader ${PROJECT_NAME}_analysis - ${PROJECT_NAME}_target_finders_opencv) + ${PROJECT_NAME}_target_finders_opencv + ${PCL_LIBRARIES}) # Add extrinsic hand-eye calibration application add_executable(${PROJECT_NAME}_extrinsic_hand_eye_calibration_app src/app/extrinsic_hand_eye_calibration_app.cpp) target_link_libraries(${PROJECT_NAME}_extrinsic_hand_eye_calibration_app PRIVATE ${PROJECT_NAME}_gui) +# Add extrinsic hand-eye calibration application +add_executable(${PROJECT_NAME}_extrinsic_hand_eye_calibration_3d_app src/app/extrinsic_hand_eye_calibration_3d_app.cpp) +target_link_libraries(${PROJECT_NAME}_extrinsic_hand_eye_calibration_3d_app PRIVATE ${PROJECT_NAME}_gui) + # Add camera intrinsic calibration application add_executable(${PROJECT_NAME}_camera_intrinsic_calibration_app src/app/camera_intrinsic_calibration_app.cpp) target_link_libraries(${PROJECT_NAME}_camera_intrinsic_calibration_app PRIVATE ${PROJECT_NAME}_gui) @@ -65,6 +74,7 @@ configure_component( TARGETS ${PROJECT_NAME}_gui ${PROJECT_NAME}_extrinsic_hand_eye_calibration_app + ${PROJECT_NAME}_extrinsic_hand_eye_calibration_3d_app ${PROJECT_NAME}_camera_intrinsic_calibration_app DEPENDENCIES "industrial_calibration COMPONENTS analysis target_finders_opencv" diff --git a/gui/include/industrial_calibration/gui/extrinsic_hand_eye_calibration_3d_widget.h b/gui/include/industrial_calibration/gui/extrinsic_hand_eye_calibration_3d_widget.h new file mode 100644 index 00000000..16491404 --- /dev/null +++ b/gui/include/industrial_calibration/gui/extrinsic_hand_eye_calibration_3d_widget.h @@ -0,0 +1,78 @@ +#pragma once + +#include + +#include +#include +#include +#include + +class QTreeWidgetItem; + +namespace Ui +{ +class ExtrinsicHandEyeCalibration3D; +} + +namespace industrial_calibration +{ +class ExtrinsicHandEyeResult; +class TargetFinderWidget; +class CameraIntrinsicsWidget; +class TransformGuess; + +class ExtrinsicHandEyeCalibration3DWidget : public QMainWindow +{ +public: + explicit ExtrinsicHandEyeCalibration3DWidget(QWidget* parent = nullptr); + ~ExtrinsicHandEyeCalibration3DWidget(); + + /** + * @brief Loads the calibration configuration from file + * @throws Exception on failure + */ + void loadConfig(const std::string& config_file); + + /** + * @brief Loads the calibration observations from file + * @throws Exception on failure + */ + void loadObservations(const std::string& observations_file); + + /** + * @brief Performs the calibration + * @throws Exception on failure + */ + void calibrate(); + + /** + * @brief Saves results of the calibration + * @throws Exception on failure + */ + void saveResults(const std::string& file); + +protected: + void closeEvent(QCloseEvent* event) override; + + void onLoadConfig(); + void onLoadObservations(); + void onCalibrate(); + void onSaveResults(); + + void loadTargetFinder(); + void drawImage(QTreeWidgetItem* item, int col); + + Ui::ExtrinsicHandEyeCalibration3D* ui_; + TargetFinderWidget* target_finder_widget_; + CameraIntrinsicsWidget* camera_intrinsics_widget_; + TransformGuess* camera_transform_guess_widget_; + TransformGuess* target_transform_guess_widget_; + + boost_plugin_loader::PluginLoader loader_; + TargetFinderFactoryOpenCV::ConstPtr factory_; + + std::shared_ptr result_; + TargetFinderOpenCV::ConstPtr target_finder_; +}; + +} // namespace industrial_calibration diff --git a/gui/src/app/extrinsic_hand_eye_calibration_3d_app.cpp b/gui/src/app/extrinsic_hand_eye_calibration_3d_app.cpp new file mode 100644 index 00000000..2a96403f --- /dev/null +++ b/gui/src/app/extrinsic_hand_eye_calibration_3d_app.cpp @@ -0,0 +1,79 @@ +#include +#include +#include +#include +#include +#include + +void handleSignal(int /*sig*/) { QApplication::instance()->quit(); } + +int main(int argc, char** argv) +{ + QApplication app(argc, argv); + + signal(SIGINT, handleSignal); + signal(SIGTERM, handleSignal); + + // Create the calibration main widget + industrial_calibration::ExtrinsicHandEyeCalibration3DWidget w; + w.setWindowTitle("Extrinsic Hand Eye Calibration 3D"); + w.setWindowIcon(QIcon(":/icons/icon.jpg")); + + // Attempt to run headless if the configuration file (argv[1]), observation file (argv[2]), and results file (argv[3]) + // are specified + if (argc > 3) + { + try + { + w.loadConfig(argv[1]); + w.loadObservations(argv[2]); + w.calibrate(); + w.saveResults(argv[3]); + QMessageBox::StandardButton ret = QMessageBox::question(nullptr, "Calibration", + "Successfully completed calibration and saved results. " + "View results in the GUI?"); + if (ret == QMessageBox::StandardButton::Yes) + { + w.showMaximized(); + return app.exec(); + } + } + catch (const std::exception& ex) + { + QString question; + QTextStream ss(&question); + ss << "Error: " << ex.what() << "\n\nOpen GUI to fix?"; + + QMessageBox::StandardButton ret = QMessageBox::question(nullptr, "Error", question); + if (ret == QMessageBox::StandardButton::Yes) + { + w.show(); + return app.exec(); + } + } + } + else + { + w.showMaximized(); + + // Attempt to load configuration and observations files if available + try + { + if (argc > 1 && std::filesystem::is_regular_file(argv[1])) + { + w.loadConfig(argv[1]); + QMessageBox::information(nullptr, "Configuration", "Successfully loaded calibration configuration"); + } + if (argc > 2 && std::filesystem::is_regular_file(argv[2])) + w.loadObservations(argv[2]); + } + catch (const std::exception& ex) + { + QMessageBox::warning(nullptr, "Error", ex.what()); + } + + return app.exec(); + } + + return 0; +} diff --git a/gui/src/extrinsic_hand_eye_calibration_3d_widget.cpp b/gui/src/extrinsic_hand_eye_calibration_3d_widget.cpp new file mode 100644 index 00000000..13b17e35 --- /dev/null +++ b/gui/src/extrinsic_hand_eye_calibration_3d_widget.cpp @@ -0,0 +1,498 @@ +#include "ui_extrinsic_hand_eye_calibration_3d_widget.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +// Analysis +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include "point_cloud_circle_finder.hpp" + +static const unsigned RANDOM_SEED = 1; +static const int IMAGE_FILE_NAME_ROLE = Qt::UserRole + 1; +static const int POSE_FILE_NAME_ROLE = Qt::UserRole + 2; +static const int CLOUD_FILE_NAME_ROLE = Qt::UserRole + 3; +static const int IDX_FEATURES = 0; +static const int IDX_HOMOGRAPHY = 1; + +static QPixmap toQt(const cv::Mat& image) +{ + return QPixmap::fromImage(QImage(image.data, image.cols, image.rows, image.step, QImage::Format_RGB888).rgbSwapped()); +} + +/** @brief helper function for showing an info message with a tree item */ +static void info(QTreeWidgetItem* item, const QString& message) +{ + item->setText(1, message); + item->setForeground(1, QApplication::palette().text()); +} + +/** @brief helper function for showing an error message with a tree item */ +static void error(QTreeWidgetItem* item, const QString& message) +{ + item->setText(1, message); + item->setForeground(1, QBrush(QColor("red"))); +} + +namespace industrial_calibration +{ +ExtrinsicHandEyeCalibration3DWidget::ExtrinsicHandEyeCalibration3DWidget(QWidget* parent) + : QMainWindow(parent) + , ui_(new Ui::ExtrinsicHandEyeCalibration3D()) + , target_finder_widget_(new TargetFinderWidget(this)) + , camera_intrinsics_widget_(new CameraIntrinsicsWidget(this)) + , camera_transform_guess_widget_(new TransformGuess(this)) + , target_transform_guess_widget_(new TransformGuess(this)) +{ + ui_->setupUi(this); + + // Configure the tree widget + ui_->tree_widget_observations->setEditTriggers(QAbstractItemView::NoEditTriggers); + + // Set up the target finder widget + { + auto dialog = new QDialog(this); + auto layout = new QVBoxLayout(dialog); + layout->addWidget(target_finder_widget_); + dialog->setWindowTitle("Edit target finder"); + + connect(ui_->action_target_finder, &QAction::triggered, dialog, &QWidget::show); + } + + // Set up the camera intrinsics widget + { + auto dialog = new QDialog(this); + auto layout = new QVBoxLayout(dialog); + layout->addWidget(camera_intrinsics_widget_); + dialog->setWindowTitle("Edit camera intrinsics"); + + connect(ui_->action_camera_intrinsics, &QAction::triggered, dialog, &QWidget::show); + } + + // Set up the camera mount to camera transform guess widget + { + auto dialog = new QDialog(this); + auto layout = new QVBoxLayout(dialog); + layout->addWidget(camera_transform_guess_widget_); + dialog->setWindowTitle("Edit camera mount to camera transform guess"); + + connect(ui_->action_camera_mount_to_camera, &QAction::triggered, dialog, &QWidget::show); + } + + // Set up the target mount to target transform guess widget + { + auto dialog = new QDialog(this); + auto layout = new QVBoxLayout(dialog); + layout->addWidget(target_transform_guess_widget_); + dialog->setWindowTitle("Edit target mount to target transform guess"); + + connect(ui_->action_target_mount_to_target, &QAction::triggered, dialog, &QWidget::show); + } + + // Set the stretch factors of the horizontal splitter to make the proportions reasonable + ui_->splitter_horizontal->setStretchFactor(0, 1); + ui_->splitter_horizontal->setStretchFactor(1, 30); + + // Move the text edit scroll bar to the maximum limit whenever it is resized + connect(ui_->text_edit_results->verticalScrollBar(), &QScrollBar::rangeChanged, [this]() { + ui_->text_edit_results->verticalScrollBar()->setSliderPosition( + ui_->text_edit_results->verticalScrollBar()->maximum()); + }); + + // Set up push buttons + connect(ui_->action_load_configuration, &QAction::triggered, this, &ExtrinsicHandEyeCalibration3DWidget::onLoadConfig); + connect(ui_->action_calibrate, &QAction::triggered, this, &ExtrinsicHandEyeCalibration3DWidget::onCalibrate); + connect(ui_->action_save, &QAction::triggered, this, &ExtrinsicHandEyeCalibration3DWidget::onSaveResults); + connect(ui_->action_load_data, &QAction::triggered, this, &ExtrinsicHandEyeCalibration3DWidget::onLoadObservations); + connect(ui_->tree_widget_observations, &QTreeWidget::itemClicked, this, + &ExtrinsicHandEyeCalibration3DWidget::drawImage); + + // Set up the plugin loader + loader_.search_libraries.insert(INDUSTRIAL_CALIBRATION_PLUGIN_LIBRARIES); + loader_.search_libraries_env = INDUSTRIAL_CALIBRATION_SEARCH_LIBRARIES_ENV; +} + +ExtrinsicHandEyeCalibration3DWidget::~ExtrinsicHandEyeCalibration3DWidget() { delete ui_; } + +void ExtrinsicHandEyeCalibration3DWidget::closeEvent(QCloseEvent* event) +{ + QMessageBox::StandardButton ret = QMessageBox::question(this, "Exit", "Are you sure you want to exit?"); + switch (ret) + { + case QMessageBox::StandardButton::No: + event->ignore(); + break; + default: + event->accept(); + } +} + +void ExtrinsicHandEyeCalibration3DWidget::onLoadConfig() +{ + try + { + // Get yaml filepath + const QString config_file = QFileDialog::getOpenFileName(this, "Load calibration configuration file", QString(), + "YAML files (*.yaml *.yml)"); + if (config_file.isNull() || config_file.isEmpty()) + return; + + loadConfig(config_file.toStdString()); + + QMessageBox::information(this, "Configuration", "Successfully loaded calibration configuration"); + } + catch (const std::exception& ex) + { + QMessageBox::warning(this, "Error", ex.what()); + } +} + +void ExtrinsicHandEyeCalibration3DWidget::loadConfig(const std::string& config_file) +{ + // Load all of the configurations before setting GUI items + YAML::Node node = YAML::LoadFile(config_file); + auto target_finder_config = getMember(node, "target_finder"); + auto intrinsics = getMember(node, "intrinsics"); + auto camera_mount_to_camera = getMember(node, "camera_mount_to_camera_guess"); + auto target_mount_to_target = getMember(node, "target_mount_to_target_guess"); + auto homography_threshold = getMember(node, "homography_threshold"); + auto static_camera = getMember(node, "static_camera"); + + target_finder_widget_->configure(target_finder_config); + camera_intrinsics_widget_->configure(intrinsics); + camera_transform_guess_widget_->configure(camera_mount_to_camera); + target_transform_guess_widget_->configure(target_mount_to_target); + ui_->double_spin_box_homography_threshold->setValue(homography_threshold); + ui_->action_static_camera->setChecked(static_camera); +} + +void ExtrinsicHandEyeCalibration3DWidget::loadTargetFinder() +{ + // Get target type and currentconfig + YAML::Node target_finder_config = target_finder_widget_->save(); + auto target_type = getMember(target_finder_config, "type"); + + factory_ = loader_.createInstance(target_type); + target_finder_ = factory_->create(target_finder_config); +} + +void ExtrinsicHandEyeCalibration3DWidget::onLoadObservations() +{ + try + { + QString observations_file = + QFileDialog::getOpenFileName(this, "Load calibration observation file", QString(), "YAML files (*.yaml *.yml)"); + if (observations_file.isNull() || observations_file.isEmpty()) + return; + + loadObservations(observations_file.toStdString()); + } + catch (const std::exception& ex) + { + ui_->tree_widget_observations->clear(); + QMessageBox::warning(this, "Error", ex.what()); + } +} + +void ExtrinsicHandEyeCalibration3DWidget::loadObservations(const std::string& observations_file) +{ + QFileInfo observations_file_info(QString::fromStdString(observations_file)); + + auto observations = getMember(YAML::LoadFile(observations_file), "data"); + + // Reset the tree widget + ui_->tree_widget_observations->clear(); + + for (std::size_t i = 0; i < observations.size(); ++i) + { + const YAML::Node& entry = observations[i]; + + QString image_file = + observations_file_info.absoluteDir().filePath(QString::fromStdString(getMember(entry, "image"))); + QString pose_file = + observations_file_info.absoluteDir().filePath(QString::fromStdString(getMember(entry, "pose"))); + QString cloud_file = + observations_file_info.absoluteDir().filePath(QString::fromStdString(getMember(entry, "cloud"))); + + auto item = new QTreeWidgetItem(); + item->setData(0, Qt::EditRole, "Observation " + QString::number(i)); + item->setData(0, IMAGE_FILE_NAME_ROLE, image_file); + item->setData(0, POSE_FILE_NAME_ROLE, pose_file); + item->setData(0, CLOUD_FILE_NAME_ROLE, cloud_file); + + // Add a column entry for the number of detected features + auto features_item = new QTreeWidgetItem(item); + features_item->setData(0, Qt::EditRole, "Feature count"); + info(features_item, "-"); + + // Add a column entry for the homography error + auto homography_item = new QTreeWidgetItem(item); + homography_item->setData(0, Qt::EditRole, "Homography error (m)"); + info(homography_item, "-"); + + ui_->tree_widget_observations->addTopLevelItem(item); + } + + ui_->tree_widget_observations->resizeColumnToContents(0); +} + +void ExtrinsicHandEyeCalibration3DWidget::drawImage(QTreeWidgetItem* item, int col) +{ + if (item == nullptr) + return; + + // Extract the top level item + while (item->parent() != nullptr) + item = item->parent(); + + QTreeWidgetItem* features_item = item->child(IDX_FEATURES); + QTreeWidgetItem* homography_item = item->child(IDX_HOMOGRAPHY); + + QString image_file = item->data(0, IMAGE_FILE_NAME_ROLE).value(); + if (!QFile(image_file).exists()) + { + error(item, "Image file does not exist"); + return; + } + + QString cloud_file = item->data(0, CLOUD_FILE_NAME_ROLE).value(); + if (!QFile(cloud_file).exists()) + { + error(item, "Cloud file does not exist"); + return; + } + + try + { + loadTargetFinder(); + cv::Mat image = cv::imread(image_file.toStdString()); + + // Detect the target in the image + TargetFeatures2D features = target_finder_->findTargetFeatures(image); + cv::Mat detected_image = target_finder_->drawTargetFeatures(image, features); + info(item, "Successfully identified target"); + + // Save the number of detected features to the tree + info(features_item, QString::number(features.size())); + + // Set the image + ui_->image_label->setPixmap(toQt(detected_image)); + update(); + + // Compute the homography error + Correspondence2D3D::Set corrs = target_finder_->target().createCorrespondences(features); + + pcl::PointCloud cloud; + pcl::io::loadPCDFile(cloud_file.toStdString(), cloud); + Correspondence3D3D::Set corrs_3d = get3DCorrespondences(cloud, corrs, image.size[1]); + + RandomCorrespondenceSampler random_sampler(corrs_3d.size(), corrs_3d.size() / 3, RANDOM_SEED); + Eigen::VectorXd homography_error = calculateHomographyError(corrs_3d, random_sampler); + double homography_error_mean = homography_error.array().mean(); + + // Save the homography error to the tree + info(homography_item, QString::number(homography_error_mean)); + + // Check homography threshold and update notes/row color + if (homography_error_mean > ui_->double_spin_box_homography_threshold->value()) + error(item, "Homography threshold violated"); + } + catch (const std::exception& ex) + { + cv::Mat image = cv::imread(image_file.toStdString()); + ui_->image_label->setPixmap(toQt(image)); + update(); + + error(item, QString(ex.what())); + info(features_item, "-"); + info(homography_item, "-"); + } +} + +void ExtrinsicHandEyeCalibration3DWidget::onCalibrate() +{ + try + { + QApplication::setOverrideCursor(Qt::WaitCursor); + calibrate(); + QApplication::restoreOverrideCursor(); + + if (result_->converged) + QMessageBox::information(this, "Calibration", "Successfully completed calibration"); + else + QMessageBox::warning(this, "Error", "Calibration failed to converge"); + } + catch (const std::exception& ex) + { + result_ = nullptr; + QApplication::restoreOverrideCursor(); + QMessageBox::warning(this, "Error", ex.what()); + } +} + +void ExtrinsicHandEyeCalibration3DWidget::calibrate() +{ + // Check if there are any observations before continuing + if (ui_->tree_widget_observations->topLevelItemCount() == 0) + throw std::runtime_error("Please load the calibration observations before performing the calibration"); + + // Load the target finder + loadTargetFinder(); + + // Create the calibration problem + ExtrinsicHandEyeProblem3D3D problem; + problem.camera_mount_to_camera_guess = camera_transform_guess_widget_->save().as(); + problem.target_mount_to_target_guess = target_transform_guess_widget_->save().as(); + + for (int i = 0; i < ui_->tree_widget_observations->topLevelItemCount(); ++i) + { + // Extract the tree items + QTreeWidgetItem* item = ui_->tree_widget_observations->topLevelItem(i); + QTreeWidgetItem* features_item = item->child(IDX_FEATURES); + QTreeWidgetItem* homography_item = item->child(IDX_HOMOGRAPHY); + + QString image_file = item->data(0, IMAGE_FILE_NAME_ROLE).value(); + if (!QFile(image_file).exists()) + { + error(item, "Image file does not exist"); + continue; + } + + QString pose_file = item->data(0, POSE_FILE_NAME_ROLE).value(); + if (!QFile(pose_file).exists()) + { + error(item, "Pose file does not exist"); + continue; + } + + QString cloud_file = item->data(0, CLOUD_FILE_NAME_ROLE).value(); + if (!QFile(cloud_file).exists()) + { + error(item, "Cloud file does not exist"); + continue; + } + + try + { + // Load the image and pose + cv::Mat image = cv::imread(image_file.toStdString()); + auto pose = YAML::LoadFile(pose_file.toStdString()).as(); + + pcl::PointCloud cloud; + pcl::io::loadPCDFile(cloud_file.toStdString(), cloud); + + // Populate an observation + Observation3D3D obs; + if (ui_->action_static_camera->isChecked()) + { + obs.to_camera_mount = Eigen::Isometry3d::Identity(); + obs.to_target_mount = pose; + } + else + { + obs.to_camera_mount = pose; + obs.to_target_mount = Eigen::Isometry3d::Identity(); + } + + Correspondence2D3D::Set corrs = target_finder_->findCorrespondences(image); + obs.correspondence_set = get3DCorrespondences(cloud, corrs, image.size[1]); + + // Calculate homography error + RandomCorrespondenceSampler random_sampler(obs.correspondence_set.size(), obs.correspondence_set.size() / 3, + RANDOM_SEED); + Eigen::VectorXd homography_error = calculateHomographyError(obs.correspondence_set, random_sampler); + double homography_error_mean = homography_error.array().mean(); + + // Conditionally add the observation to the problem if the mean homography error is less than the threshold + if (homography_error_mean < ui_->double_spin_box_homography_threshold->value()) + { + problem.observations.push_back(obs); + info(item, "Included in calibration"); + } + else + { + // Update the notes + error(item, "Excluded from calibration (homography threshold violation)"); + } + + // Update the tree widget + info(features_item, QString::number(obs.correspondence_set.size())); + info(homography_item, QString::number(homography_error_mean)); + } + catch (const std::exception& ex) + { + error(item, QString(ex.what())); + } + } + + // Solve the calibration problem + result_ = std::make_shared(); + *result_ = optimize(problem); + + // Report results + std::stringstream ss; + ss << *result_ << std::endl; + ss << result_->covariance.printCorrelationCoeffAboveThreshold(0.5) << std::endl; + + // Compute the projected 3D error for comparison + // ss << analyze3dProjectionError(problem, *result_) << std::endl << std::endl; + + // Now let's compare the results of our extrinsic calibration with a PnP optimization for every observation. + // The PnP optimization will give us an estimate of the camera to target transform using our input camera intrinsic + // parameters We will then see how much this transform differs from the same transform calculated using the results + // of the extrinsic calibration + // ExtrinsicHandEyeAnalysisStats stats = analyzeResults(problem, *result_); + // ss << stats << std::endl << std::endl; + + ui_->text_edit_results->clear(); + ui_->text_edit_results->append(QString::fromStdString(ss.str())); +} + +void ExtrinsicHandEyeCalibration3DWidget::onSaveResults() +{ + try + { + const QString file = QFileDialog::getSaveFileName(this, QString(), QString(), "YAML files (*.yaml *.yml)"); + if (file.isNull() || file.isEmpty()) + return; + + saveResults(file.toStdString()); + } + catch (const std::exception& ex) + { + QMessageBox::warning(this, "Error", ex.what()); + } +} + +void ExtrinsicHandEyeCalibration3DWidget::saveResults(const std::string& file) +{ + if (result_ == nullptr) + throw ICalException("Calibration problem has not yet been solved. Please load the calibration data and run the " + "calibration"); + + std::ofstream f(file); + f << YAML::Node(*result_); +} + +} // namespace industrial_calibration diff --git a/gui/src/extrinsic_hand_eye_calibration_3d_widget.ui b/gui/src/extrinsic_hand_eye_calibration_3d_widget.ui new file mode 100644 index 00000000..99d47353 --- /dev/null +++ b/gui/src/extrinsic_hand_eye_calibration_3d_widget.ui @@ -0,0 +1,317 @@ + + + ExtrinsicHandEyeCalibration3D + + + + 0 + 0 + 429 + 558 + + + + MainWindow + + + + + + + Qt::Horizontal + + + + Qt::Vertical + + + + Observations + + + + + + + + 4 + + + 0.005000000000000 + + + + + + + Homography Threshold (m) + + + + + + + + + true + + + Qt::ElideRight + + + false + + + true + + + true + + + true + + + + Observation + + + + + Notes + + + + + + + + + Results + + + + + + true + + + + + + + + + QFrame::StyledPanel + + + QFrame::Raised + + + + + + + 0 + 0 + + + + <html><head/><body><p><span style=" font-weight:700; text-decoration: underline;">Extrinsic Hand-Eye Calibration</span></p><p><br/></p><p>Configure the calibration, either from a YAML file (<span style=" font-style:italic;">Load Configuration</span> button) or manually using the icons</p><p>Load the calibration observations from a YAML file using the <span style=" font-style:italic;">Load Observations</span> button</p><p>Click on an observation in the list to view the image and detected target</p><p>Click the <span style=" font-style:italic;">Calibrate</span> button to perform the calibration</p><p>Click the <span style=" font-style:italic;">Save</span> button to save the calibration results to file</p></body></html> + + + Qt::AlignCenter + + + + + + + + + + + + + 0 + 0 + 429 + 22 + + + + + Edit + + + + Initial guesses + + + + + + + + + + + + Calibration + + + + + + File + + + + + + + + + + + + + toolBar + + + TopToolBarArea + + + false + + + + + + + + + + + + + + + + + + + Load configuration + + + Load calibration configuration file + + + + + + + + Target finder + + + Edit target finder + + + + + + + + Load observations + + + Load observations file + + + + + + + + Calibrate + + + Run calibration + + + + + + + + Save calibration + + + Save calibration + + + + + true + + + + + + Static camera + + + Enable if camera is statically mounted + + + + + + :/icons/frame_camera.png:/icons/frame_camera.png + + + Camera mount to camera + + + Edit camera mount to camera transform guess + + + + + + :/icons/frame_target.png:/icons/frame_target.png + + + Target mount to target + + + Edit target mount to target transform guess + + + + + + + + Camera intrinsics + + + Edit camera intrinsics + + + + + + industrial_calibration::AspectRatioPixmapLabel + QLabel +
industrial_calibration/gui/aspect_ratio_pixmap_label.h
+ + + + tree_widget_observations + double_spin_box_homography_threshold + text_edit_results + + + + diff --git a/gui/src/point_cloud_circle_finder.hpp b/gui/src/point_cloud_circle_finder.hpp new file mode 100644 index 00000000..b1e88a5e --- /dev/null +++ b/gui/src/point_cloud_circle_finder.hpp @@ -0,0 +1,127 @@ +#pragma once + +#include + +#include +#include +#include + +namespace industrial_calibration +{ +/** + * @brief Gets the weights of the four pixels surrounding a floating point location in an image based on the XY + * distance of the pixels to the feature + * @details Returns the weights for the points starting with the upper-left pixel and moving clockwise + */ +Eigen::Vector4d getWeightedAverageWeights(const double row, const double col) +{ + // Separate the decimal component from the rest of the float, such that (dx, dy) represents the vector from the upper + // left pixel to the feature + double o_row, o_col; + const double d_row = std::modf(row, &o_row); + const double d_col = std::modf(col, &o_col); + + // Calculate the vector normal from each of the four pixels nearest to the feature, starting from the upper left and + // moving clockwise. The weight is the inverse of this distance: smaller distance -> larger weight + Eigen::Vector4d out; + out[0] = 1.0 / Eigen::Vector2d(0.0 - d_row, 0.0 - d_col).norm(); + out[1] = 1.0 / Eigen::Vector2d(1.0 - d_row, 0.0 - d_col).norm(); + out[2] = 1.0 / Eigen::Vector2d(1.0 - d_row, 1.0 - d_col).norm(); + out[3] = 1.0 / Eigen::Vector2d(0.0 - d_row, 1.0 - d_col).norm(); + + // Return the normalized weights, such that they sum to 1 + return out / out.sum(); +} + +Correspondence3D3D::Set get3DCorrespondences( + const pcl::PointCloud& cloud, const Correspondence2D3D::Set& correspondences, int width) +{ + // Initialize the output variable + Correspondence3D3D::Set out; + out.reserve(correspondences.size()); + + // Loop over each observation and calculate the 3D point from the point cloud that most closely matches the center of + // the circle observation + for (const Correspondence2D3D& corr : correspondences) + { + // Get the center of the circle + const double col = corr.in_image.x(); + const double row = corr.in_image.y(); + + // Assumption: the point cloud is organized and each point cloud point corresponds to a pixel + // Since the center of the circle is a real number (i.e. a double) there is a discrete pixel on both sides of that + // point in both X and Y Let's get the index of those 4 surrounding pixels + const auto col_lower = static_cast(std::floor(col)); + const auto row_lower = static_cast(std::floor(row)); + const auto col_upper = static_cast(std::ceil(col)); + const auto row_upper = static_cast(std::ceil(row)); + + // Get the weights for a weighted average of these four points based on their distance from the observed feature in + // image space + Eigen::Vector4d weights = getWeightedAverageWeights(row, col); + + /* Initialize variables to hold the 3D locations of the 4 pixels in a column-wise matrix + * Points: + * ul (upper left, corresponds to the pixel indices x (column) low, y (row) low) + * ur (upper right, corresponds to the pixel indices x (column) high, y (row) low) + * lr (lower_right, corresponds to the pixel indices x (column) high, y (row) high) + * ll (lower left, corresponds to the pixel indices x (column) low, y (row) high) + */ + Eigen::Matrix pts; + for (std::size_t i = 0; i < 4; ++i) + { + int idx; + switch (i) + { + case 0: + idx = row_lower * width + col_lower; + break; + case 1: + idx = row_lower * width + col_upper; + break; + case 2: + idx = row_upper * width + col_upper; + break; + case 3: + idx = row_upper * width + col_lower; + break; + default: + throw std::runtime_error("Invalid case value for point creation"); + } + pts.col(i) = cloud.at(idx).getArray3fMap().cast(); + } + + // Create a mask for a weighted average based on whether the point has finite values and the point norm is non-zero + Eigen::Array finite_mask = pts.array().isFinite().colwise().all(); + Eigen::Array nonzero_mask = pts.colwise().norm().array() > 0.001; + Eigen::Array mask = finite_mask && nonzero_mask; + + // Skip if all the points are masked + if (mask.count() == 0) + { + std::cout << "No valid points found around feature" << std::endl; + continue; + } + + // Apply the boolean mask and renormalize the weights so they sum to 1 + weights.array() *= mask.cast(); + weights /= weights.sum(); + + Correspondence3D3D corr_3d; + corr_3d.in_target = corr.in_target; + + // Apply the weighted average to the points + corr_3d.in_image = pts * weights; + // corr_3d.in_image = cloud.at(row_lower * width + col_lower).getArray3fMap().cast(); + + Eigen::IOFormat fmt(4, 0, ",", "\n", "[", "]"); + std::cout << "Point: " << corr_3d.in_image.transpose().format(fmt) << " (weighted average of " << mask.count() + << "/4 nearest points)" << std::endl; + + out.push_back(corr_3d); + } + + return out; +} + +} // namespace industrial_calibration