Skip to content

Commit

Permalink
Added serialization to ROS format for camera intrinsic calibration app
Browse files Browse the repository at this point in the history
  • Loading branch information
@marip8
marip8 committed Jun 13, 2024
1 parent 4ed9618 commit 58034a0
Show file tree
Hide file tree
Showing 4 changed files with 147 additions and 35 deletions.
10 changes: 9 additions & 1 deletion gui/include/industrial_calibration/gui/camera_intrinsic_calibration_widget.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,13 +48,21 @@ class CameraIntrinsicCalibrationWidget : public QMainWindow
* @brief Saves the calibration results
* @throws Exception on failure
*/
void saveResults(const std::string& file);
void saveResults(const std::string& file) const;

/**
* @brief Saves the calibration results to a YAML file in a format compatible with ROS
* @details Format definition https://wiki.ros.org/camera_calibration_parsers#File_formats
* @throws Exception on failure
*/
void saveROSFormat(const std::string& file) const;

private:
void onLoadConfig();
void onLoadObservations();
void onCalibrate();
void onSaveResults();
void onSaveROSFormat();

void loadTargetFinder();
void drawImage(QTreeWidgetItem* item, int col);
Expand Down
10 changes: 8 additions & 2 deletions gui/src/app/camera_intrinsic_calibration_app.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,8 +18,12 @@ int main(int argc, char** argv)
w.setWindowTitle("Camera Intrinsic Calibration");
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
/* Attempt to run headless if all files are specified:
* argv[1]: configuration file
* argv[2]: observation file
* argv[3]: results file (industrial_calibration)
* argv[4]: results file (ROS) - optional
*/
if (argc > 3)
{
try
Expand All @@ -28,6 +32,8 @@ int main(int argc, char** argv)
w.loadObservations(argv[2]);
w.calibrate();
w.saveResults(argv[3]);
if (argc > 4) w.saveROSFormat(argv[4]);

QMessageBox::StandardButton ret = QMessageBox::question(nullptr, "Calibration",
"Successfully completed calibration and saved results. "
"View results in the GUI?");
Expand Down
93 changes: 92 additions & 1 deletion gui/src/camera_intrinsic_calibration_widget.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -167,6 +167,7 @@ CameraIntrinsicCalibrationWidget::CameraIntrinsicCalibrationWidget(QWidget* pare
connect(ui_->action_load_data, &QAction::triggered, this, &CameraIntrinsicCalibrationWidget::onLoadObservations);
connect(ui_->action_calibrate, &QAction::triggered, this, &CameraIntrinsicCalibrationWidget::onCalibrate);
connect(ui_->action_save, &QAction::triggered, this, &CameraIntrinsicCalibrationWidget::onSaveResults);
connect(ui_->action_save_ros_format, &QAction::triggered, this, &CameraIntrinsicCalibrationWidget::onSaveROSFormat);
connect(ui_->tree_widget_observations, &QTreeWidget::itemClicked, this, &CameraIntrinsicCalibrationWidget::drawImage);

// Set up the plugin loader
Expand Down Expand Up @@ -485,7 +486,7 @@ void CameraIntrinsicCalibrationWidget::onSaveResults()
}
}

void CameraIntrinsicCalibrationWidget::saveResults(const std::string& file)
void CameraIntrinsicCalibrationWidget::saveResults(const std::string& file) const
{
if (result_ == nullptr)
throw ICalException("Calibration problem has not yet been solved. Please load the calibration data and run the "
Expand All @@ -495,4 +496,94 @@ void CameraIntrinsicCalibrationWidget::saveResults(const std::string& file)
f << YAML::Node(*result_);
}

void CameraIntrinsicCalibrationWidget::onSaveROSFormat()
{
try
{
const QString file = QFileDialog::getSaveFileName(this, QString(), QString(), "YAML files (*.yaml *.yml)");
if (file.isNull() || file.isEmpty()) return;

saveROSFormat(file.toStdString());
}
catch (const std::exception& ex)
{
QMessageBox::warning(this, "Error", ex.what());
}
}

void CameraIntrinsicCalibrationWidget::saveROSFormat(const std::string& file) const
{
if (result_ == nullptr) return;

Eigen::Matrix<double, 3, 4, Eigen::RowMajor> mat;
mat << result_->intrinsics.fx(), 0.0, result_->intrinsics.cx(), 0.0, 0.0, result_->intrinsics.fy(),
result_->intrinsics.cy(), 0.0, 0.0, 0.0, 1.0, 0.0;

YAML::Node node;

node["camera_name"] = "";

// Image size
{
if (ui_->tree_widget_observations->topLevelItemCount() == 0) throw ICalException("No observations have been added");

QTreeWidgetItem* item = ui_->tree_widget_observations->topLevelItem(0);
QString image_file = item->data(0, IMAGE_FILE_NAME_ROLE).value<QString>();
if (!QFile(image_file).exists()) throw ICalException("Image '" + image_file.toStdString() + "' does not exist");

cv::Mat image = cv::imread(image_file.toStdString());
node["image_height"] = image.size[0];
node["image_width"] = image.size[1];
}

// Camera matrix
{
Eigen::Matrix<double, 3, 3, Eigen::RowMajor> intr = mat.block<3, 3>(0, 0);

YAML::Node camera_matrix;
camera_matrix["rows"] = 3;
camera_matrix["cols"] = 3;
camera_matrix["data"] = std::vector<double>(intr.data(), intr.data() + 9);

node["camera_matrix"] = camera_matrix;
}

// Distortion
{
YAML::Node distortion;
distortion["rows"] = 1;
distortion["cols"] = 5;
distortion = result_->distortions;

node["distortion_model"] = "plumb_bob";
node["distortion_coefficients"] = distortion;
}

// Rectification matrix
{
Eigen::Matrix<double, 3, 3, Eigen::RowMajor> identity;
identity.setIdentity();

YAML::Node rect_matrix;
rect_matrix["rows"] = 3;
rect_matrix["cols"] = 3;
rect_matrix["data"] = std::vector<double>(identity.data(), identity.data() + 9);

node["rectification_matrix"] = rect_matrix;
}

// Projection matrix
{
YAML::Node proj_matrix;
proj_matrix["rows"] = 3;
proj_matrix["cols"] = 4;
proj_matrix["data"] = std::vector<double>(mat.data(), mat.data() + 12);

node["projection_matrix"] = proj_matrix;
}

std::ofstream f(file);
f << node;
}

} // namespace industrial_calibration
69 changes: 38 additions & 31 deletions gui/src/camera_intrinsic_calibration_widget.ui
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@
<rect>
<x>0</x>
<y>0</y>
<width>429</width>
<width>641</width>
<height>558</height>
</rect>
</property>
Expand Down Expand Up @@ -131,41 +131,45 @@
<rect>
<x>0</x>
<y>0</y>
<width>429</width>
<width>641</width>
<height>22</height>
</rect>
</property>
<widget class="QMenu" name="menuEdit">
<property name="title">
<string>Edit</string>
</property>
<widget class="QMenu" name="menuInitial_Guesses">
<property name="title">
<string>Initial guesses</string>
</property>
</widget>
<property name="toolTipsVisible">
<bool>true</bool>
</property>
<addaction name="action_edit_target_finder"/>
<addaction name="action_camera_intrinsics"/>
<addaction name="menuInitial_Guesses"/>
<addaction name="action_use_extrinsic_guesses"/>
</widget>
<widget class="QMenu" name="menuCalibrate_2">
<widget class="QMenu" name="menuCalibrate">
<property name="title">
<string>Calibration</string>
</property>
<property name="toolTipsVisible">
<bool>true</bool>
</property>
<addaction name="action_calibrate"/>
</widget>
<widget class="QMenu" name="menuFile">
<property name="title">
<string>File</string>
</property>
<property name="toolTipsVisible">
<bool>true</bool>
</property>
<addaction name="action_load_configuration"/>
<addaction name="action_load_data"/>
<addaction name="action_save"/>
<addaction name="action_save_ros_format"/>
</widget>
<addaction name="menuFile"/>
<addaction name="menuEdit"/>
<addaction name="menuCalibrate_2"/>
<addaction name="menuCalibrate"/>
</widget>
<widget class="QStatusBar" name="statusbar"/>
<widget class="QToolBar" name="toolBar">
Expand All @@ -188,23 +192,22 @@
<addaction name="separator"/>
<addaction name="action_calibrate"/>
<addaction name="action_save"/>
<addaction name="action_save_ros_format"/>
</widget>
<action name="action_load_configuration">
<property name="icon">
<iconset theme="document-open">
<normaloff>.</normaloff>.</iconset>
<iconset theme="document-open"/>
</property>
<property name="text">
<string>Load configuration</string>
<string>Load configuration...</string>
</property>
<property name="toolTip">
<string>Load calibration configuration file</string>
</property>
</action>
<action name="action_edit_target_finder">
<property name="icon">
<iconset theme="edit-find">
<normaloff>.</normaloff>.</iconset>
<iconset theme="edit-find"/>
</property>
<property name="text">
<string>Target finder</string>
Expand All @@ -215,20 +218,18 @@
</action>
<action name="action_load_data">
<property name="icon">
<iconset theme="document-new">
<normaloff>.</normaloff>.</iconset>
<iconset theme="document-new"/>
</property>
<property name="text">
<string>Load observations</string>
<string>Load observations...</string>
</property>
<property name="toolTip">
<string>Load observations file</string>
</property>
</action>
<action name="action_calibrate">
<property name="icon">
<iconset theme="media-playback-start">
<normaloff>.</normaloff>.</iconset>
<iconset theme="media-playback-start"/>
</property>
<property name="text">
<string>Calibrate</string>
Expand All @@ -239,11 +240,10 @@
</action>
<action name="action_save">
<property name="icon">
<iconset theme="document-save">
<normaloff>.</normaloff>.</iconset>
<iconset theme="document-save"/>
</property>
<property name="text">
<string>Save calibration</string>
<string>Save calibration...</string>
</property>
<property name="toolTip">
<string>Save calibration</string>
Expand All @@ -254,8 +254,7 @@
<bool>true</bool>
</property>
<property name="icon">
<iconset theme="user-available">
<normaloff>.</normaloff>.</iconset>
<iconset theme="user-available"/>
</property>
<property name="text">
<string>Use target pose guesses</string>
Expand All @@ -266,8 +265,7 @@
</action>
<action name="action_camera_intrinsics">
<property name="icon">
<iconset theme="camera-photo">
<normaloff>.</normaloff>.</iconset>
<iconset theme="camera-photo"/>
</property>
<property name="text">
<string>Camera intrinsics</string>
Expand All @@ -281,7 +279,7 @@
<bool>true</bool>
</property>
<property name="icon">
<iconset resource="../resources/resources.qrc">
<iconset>
<normaloff>:/icons/opencv.svg</normaloff>:/icons/opencv.svg</iconset>
</property>
<property name="text">
Expand All @@ -294,6 +292,17 @@
<enum>QAction::NoRole</enum>
</property>
</action>
<action name="action_save_ros_format">
<property name="icon">
<iconset theme="document-save-as"/>
</property>
<property name="text">
<string>Save calibration (ROS)...</string>
</property>
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Save the calibration results to a &lt;a href=&quot;https://wiki.ros.org/camera_calibration_parsers#File_formats&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#007af4;&quot;&gt;format comptible with ROS&lt;/span&gt;&lt;/a&gt;&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
</action>
</widget>
<customwidgets>
<customwidget>
Expand All @@ -307,8 +316,6 @@
<tabstop>double_spin_box_homography_threshold</tabstop>
<tabstop>text_edit_results</tabstop>
</tabstops>
<resources>
<include location="../resources/resources.qrc"/>
</resources>
<resources/>
<connections/>
</ui>

0 comments on commit 58034a0

Please sign in to comment.