Draft: extrinsic hand eye calibration with 3D data

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 "")
 

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 "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-                                                      "$<INSTALL_INTERFACE:include>")
+                                                      "$<INSTALL_INTERFACE:include>"
+                                                      ${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"

gui/include/industrial_calibration/gui/extrinsic_hand_eye_calibration_3d_widget.h

@@ -0,0 +1,78 @@
+#pragma once
+
+#include <industrial_calibration/target_finders/opencv/target_finder.h>
+
+#include <boost_plugin_loader/plugin_loader.h>
+#include <memory>
+#include <QMainWindow>
+#include <QDialog>
+
+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<ExtrinsicHandEyeResult> result_;
+  TargetFinderOpenCV::ConstPtr target_finder_;
+};
+
+}  // namespace industrial_calibration

gui/src/app/extrinsic_hand_eye_calibration_3d_app.cpp

@@ -0,0 +1,79 @@
+#include <industrial_calibration/gui/extrinsic_hand_eye_calibration_3d_widget.h>
+#include <filesystem>
+#include <QApplication>
+#include <QMessageBox>
+#include <QTextStream>
+#include <signal.h>
+
+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;
+}