GrabBag/Tools/CalibView/Src/CalibDataWidget.cpp

#include "CalibDataWidget.h"
#include "../../SpinBoxPasteHelper.h"

#include <QVBoxLayout>
#include <QHBoxLayout>
#include <QSettings>
#include <QDateTime>
#include <QGridLayout>
#include <QHeaderView>
#include <QLabel>
#include <array>

CalibDataWidget::CalibDataWidget(QWidget* parent)
    : QWidget(parent)
    , m_cbCalibType(nullptr)
    , m_cbEulerOrder(nullptr)
    , m_tableHandEye(nullptr)
    , m_groupHandEye(nullptr)
    , m_btnHandEyeAddRow(nullptr)
    , m_btnHandEyeDeleteRow(nullptr)
    , m_btnHandEyeCalib(nullptr)
    , m_groupTCPCalib(nullptr)
    , m_tableTCP(nullptr)
    , m_tcpModeCombo(nullptr)
    , m_tcpEulerOrderCombo(nullptr)
    , m_tcpRefPoseIndex(nullptr)
    , m_tcpWorldRx(nullptr)
    , m_tcpWorldRy(nullptr)
    , m_tcpWorldRz(nullptr)
    , m_tcpOrientationGroup(nullptr)
    , m_tcpAddRowBtn(nullptr)
    , m_tcpRemoveRowBtn(nullptr)
    , m_btnTCPCalib(nullptr)
    , m_inputCamX(nullptr)
    , m_inputCamY(nullptr)
    , m_inputCamZ(nullptr)
    , m_inputCamRoll(nullptr)
    , m_inputCamPitch(nullptr)
    , m_inputCamYaw(nullptr)
    , m_inputRobotX(nullptr)
    , m_inputRobotY(nullptr)
    , m_inputRobotZ(nullptr)
    , m_inputRobotRoll(nullptr)
    , m_inputRobotPitch(nullptr)
    , m_inputRobotYaw(nullptr)
    , m_btnHandEyeAddInput(nullptr)
    , m_inputTcpX(nullptr)
    , m_inputTcpY(nullptr)
    , m_inputTcpZ(nullptr)
    , m_inputTcpRx(nullptr)
    , m_inputTcpRy(nullptr)
    , m_inputTcpRz(nullptr)
    , m_btnTcpAddInput(nullptr)
{
    setupUI();
    SpinBoxPasteHelper::install(this);
}

CalibDataWidget::~CalibDataWidget()
{
}

void CalibDataWidget::setupUI()
{
    QVBoxLayout* mainLayout = new QVBoxLayout(this);

    // 标定模式选择
    QHBoxLayout* modeLayout = new QHBoxLayout();
    QLabel* lblMode = new QLabel("标定模式:", this);
    m_cbCalibType = new QComboBox(this);
    m_cbCalibType->addItem("Eye-To-Hand (眼在手外)");
    m_cbCalibType->addItem("Eye-In-Hand (眼在手上)");
    m_cbCalibType->addItem("TCP 标定");
    connect(m_cbCalibType, QOverload<int>::of(&QComboBox::currentIndexChanged),
            this, &CalibDataWidget::onCalibTypeChanged);
    modeLayout->addWidget(lblMode);
    modeLayout->addWidget(m_cbCalibType);

    // 欧拉角顺序选择
    modeLayout->addSpacing(20);
    QLabel* lblEuler = new QLabel("欧拉角顺序:", this);
    m_cbEulerOrder = new QComboBox(this);
    m_cbEulerOrder->addItem("ZYX (默认)");
    m_cbEulerOrder->addItem("XYZ");
    m_cbEulerOrder->addItem("XZY");
    m_cbEulerOrder->addItem("YXZ");
    m_cbEulerOrder->addItem("YZX");
    m_cbEulerOrder->addItem("ZXY");
    modeLayout->addWidget(lblEuler);
    modeLayout->addWidget(m_cbEulerOrder);

    modeLayout->addStretch();
    mainLayout->addLayout(modeLayout);

    // 手眼标定数据组（Eye-To-Hand 和 Eye-In-Hand 共用）
    m_groupHandEye = createHandEyeGroup();
    mainLayout->addWidget(m_groupHandEye, 1);

    // TCP 标定数据组
    m_groupTCPCalib = createTCPCalibGroup();
    mainLayout->addWidget(m_groupTCPCalib, 1);
    m_groupTCPCalib->setVisible(false);

    // 初始化按钮启用状态
    onCalibTypeChanged(0);
}

QWidget* CalibDataWidget::createHandEyeGroup()
{
    QWidget* group = new QWidget(this);
    QVBoxLayout* layout = new QVBoxLayout(group);
    layout->setContentsMargins(0, 0, 0, 0);

    m_tableHandEye = new QTableWidget(this);
    m_tableHandEye->setColumnCount(12);
    m_tableHandEye->setHorizontalHeaderLabels({
        "标定板X(mm)", "标定板Y(mm)", "标定板Z(mm)", "标定板Rx(rad)", "标定板Ry(rad)", "标定板Rz(rad)",
        "机械臂X(mm)", "机械臂Y(mm)", "机械臂Z(mm)", "机械臂Rx(°)", "机械臂Ry(°)", "机械臂Rz(°)"
    });
    m_tableHandEye->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch);
    m_tableHandEye->setSelectionBehavior(QAbstractItemView::SelectRows);

    layout->addWidget(m_tableHandEye, 1);

    // 内联按钮行
    QHBoxLayout* btnLayout = new QHBoxLayout();
    m_btnHandEyeAddRow = new QPushButton("添加行", this);
    m_btnHandEyeDeleteRow = new QPushButton("删除行", this);
    m_btnHandEyeCalib = new QPushButton("手眼标定", this);

    connect(m_btnHandEyeAddRow, &QPushButton::clicked, this, [this]() {
        int row = m_tableHandEye->rowCount();
        m_tableHandEye->insertRow(row);
        for (int col = 0; col < 12; ++col) {
            m_tableHandEye->setItem(row, col, new QTableWidgetItem("0"));
        }
        m_tableHandEye->scrollToBottom();
    });
    connect(m_btnHandEyeDeleteRow, &QPushButton::clicked, this, [this]() {
        int row = m_tableHandEye->currentRow();
        if (row >= 0) {
            m_tableHandEye->removeRow(row);
        }
    });
    connect(m_btnHandEyeCalib, &QPushButton::clicked, this, [this]() {
        int mode = m_cbCalibType->currentIndex();
        if (mode == 0) {
            emit requestEyeToHandCalib();
        } else if (mode == 1) {
            emit requestEyeInHandCalib();
        }
    });

    btnLayout->addWidget(m_btnHandEyeAddRow);
    btnLayout->addWidget(m_btnHandEyeDeleteRow);
    btnLayout->addWidget(m_btnHandEyeCalib);
    btnLayout->addStretch();
    layout->addLayout(btnLayout);

    // 数据输入区
    QGroupBox* inputGroup = new QGroupBox("数据输入", group);
    QGridLayout* inputLayout = new QGridLayout(inputGroup);
    inputLayout->setHorizontalSpacing(2);
    inputLayout->setVerticalSpacing(4);
    inputLayout->setContentsMargins(4, 4, 4, 4);
    inputLayout->setColumnStretch(0, 0);
    inputLayout->setColumnStretch(1, 1);
    inputLayout->setColumnStretch(2, 0);
    inputLayout->setColumnStretch(3, 1);
    inputLayout->setColumnStretch(4, 0);
    inputLayout->setColumnStretch(5, 1);

    auto createSpinBox = [this]() {
        QDoubleSpinBox* sb = new QDoubleSpinBox(this);
        sb->setRange(-10000, 10000);
        sb->setDecimals(3);
        return sb;
    };

    // 第1行：标定板 X/Y/Z
    inputLayout->addWidget(new QLabel("标定板X(mm):", this), 0, 0);
    m_inputCamX = createSpinBox();
    inputLayout->addWidget(m_inputCamX, 0, 1);
    inputLayout->addWidget(new QLabel("Y(mm):", this), 0, 2);
    m_inputCamY = createSpinBox();
    inputLayout->addWidget(m_inputCamY, 0, 3);
    inputLayout->addWidget(new QLabel("Z(mm):", this), 0, 4);
    m_inputCamZ = createSpinBox();
    inputLayout->addWidget(m_inputCamZ, 0, 5);

    // 第2行：标定板 Rx/Ry/Rz (弧度)
    inputLayout->addWidget(new QLabel("标定板Rx(rad):", this), 1, 0);
    m_inputCamRoll = createSpinBox();
    inputLayout->addWidget(m_inputCamRoll, 1, 1);
    inputLayout->addWidget(new QLabel("Ry(rad):", this), 1, 2);
    m_inputCamPitch = createSpinBox();
    inputLayout->addWidget(m_inputCamPitch, 1, 3);
    inputLayout->addWidget(new QLabel("Rz(rad):", this), 1, 4);
    m_inputCamYaw = createSpinBox();
    inputLayout->addWidget(m_inputCamYaw, 1, 5);

    // 第3行：机械臂 X/Y/Z
    inputLayout->addWidget(new QLabel("机械臂X(mm):", this), 2, 0);
    m_inputRobotX = createSpinBox();
    inputLayout->addWidget(m_inputRobotX, 2, 1);
    inputLayout->addWidget(new QLabel("Y(mm):", this), 2, 2);
    m_inputRobotY = createSpinBox();
    inputLayout->addWidget(m_inputRobotY, 2, 3);
    inputLayout->addWidget(new QLabel("Z(mm):", this), 2, 4);
    m_inputRobotZ = createSpinBox();
    inputLayout->addWidget(m_inputRobotZ, 2, 5);

    // 第4行：机械臂 Rx/Ry/Rz (角度)
    inputLayout->addWidget(new QLabel("机械臂Rx(°):", this), 3, 0);
    m_inputRobotRoll = createSpinBox();
    inputLayout->addWidget(m_inputRobotRoll, 3, 1);
    inputLayout->addWidget(new QLabel("Ry(°):", this), 3, 2);
    m_inputRobotPitch = createSpinBox();
    inputLayout->addWidget(m_inputRobotPitch, 3, 3);
    inputLayout->addWidget(new QLabel("Rz(°):", this), 3, 4);
    m_inputRobotYaw = createSpinBox();
    inputLayout->addWidget(m_inputRobotYaw, 3, 5);

    // 第5行：添加到表格按钮
    m_btnHandEyeAddInput = new QPushButton("添加到表格", this);
    connect(m_btnHandEyeAddInput, &QPushButton::clicked, this, [this]() {
        int row = m_tableHandEye->rowCount();
        m_tableHandEye->insertRow(row);
        m_tableHandEye->setItem(row, 0, new QTableWidgetItem(QString::number(m_inputCamX->value(), 'f', 3)));
        m_tableHandEye->setItem(row, 1, new QTableWidgetItem(QString::number(m_inputCamY->value(), 'f', 3)));
        m_tableHandEye->setItem(row, 2, new QTableWidgetItem(QString::number(m_inputCamZ->value(), 'f', 3)));
        m_tableHandEye->setItem(row, 3, new QTableWidgetItem(QString::number(m_inputCamRoll->value(), 'f', 3)));
        m_tableHandEye->setItem(row, 4, new QTableWidgetItem(QString::number(m_inputCamPitch->value(), 'f', 3)));
        m_tableHandEye->setItem(row, 5, new QTableWidgetItem(QString::number(m_inputCamYaw->value(), 'f', 3)));
        m_tableHandEye->setItem(row, 6, new QTableWidgetItem(QString::number(m_inputRobotX->value(), 'f', 3)));
        m_tableHandEye->setItem(row, 7, new QTableWidgetItem(QString::number(m_inputRobotY->value(), 'f', 3)));
        m_tableHandEye->setItem(row, 8, new QTableWidgetItem(QString::number(m_inputRobotZ->value(), 'f', 3)));
        m_tableHandEye->setItem(row, 9, new QTableWidgetItem(QString::number(m_inputRobotRoll->value(), 'f', 3)));
        m_tableHandEye->setItem(row, 10, new QTableWidgetItem(QString::number(m_inputRobotPitch->value(), 'f', 3)));
        m_tableHandEye->setItem(row, 11, new QTableWidgetItem(QString::number(m_inputRobotYaw->value(), 'f', 3)));
        m_tableHandEye->scrollToBottom();
    });
    inputLayout->addWidget(m_btnHandEyeAddInput, 4, 0, 1, 6);

    layout->addWidget(inputGroup);

    return group;
}

void CalibDataWidget::onCalibTypeChanged(int index)
{
    // 更新表格可见性
    m_groupHandEye->setVisible(index == 0 || index == 1);
    m_groupTCPCalib->setVisible(index == 2);

    // 根据模式更新表格标题
    if (index == 0) {
        // Eye-To-Hand 模式
        m_tableHandEye->setHorizontalHeaderLabels({
            "Cam X", "Cam Y", "Cam Z", "Cam Roll", "Cam Pitch", "Cam Yaw",
            "Robot X", "Robot Y", "Robot Z", "Robot Roll", "Robot Pitch", "Robot Yaw"
        });
        m_btnHandEyeCalib->setText("Eye-To-Hand 标定");
    } else if (index == 1) {
        // Eye-In-Hand 模式
        m_tableHandEye->setHorizontalHeaderLabels({
            "Robot X", "Robot Y", "Robot Z", "Robot Roll", "Robot Pitch", "Robot Yaw",
            "Cam X", "Cam Y", "Cam Z", "Cam Roll", "Cam Pitch", "Cam Yaw"
        });
        m_btnHandEyeCalib->setText("Eye-In-Hand 标定");
    }

    // TCP 按钮
    m_tcpAddRowBtn->setEnabled(index == 2);
    m_tcpRemoveRowBtn->setEnabled(index == 2);
    m_btnTCPCalib->setEnabled(index == 2);

    // 发送标定类型改变信号
    if (index == 0) {
        emit calibTypeChanged(HECCalibrationType::EyeToHand);
    } else if (index == 1) {
        emit calibTypeChanged(HECCalibrationType::EyeInHand);
    }
}

void CalibDataWidget::getEyeToHandData(std::vector<HECPoint3D>& eyePoints,
                                       std::vector<HECPoint3D>& robotPoints) const
{
    eyePoints.clear();
    robotPoints.clear();

    int rowCount = m_tableHandEye->rowCount();

    for (int row = 0; row < rowCount; ++row) {
        HECPoint3D eyePoint;
        eyePoint.x = m_tableHandEye->item(row, 0) ?
            m_tableHandEye->item(row, 0)->text().toDouble() : 0;
        eyePoint.y = m_tableHandEye->item(row, 1) ?
            m_tableHandEye->item(row, 1)->text().toDouble() : 0;
        eyePoint.z = m_tableHandEye->item(row, 2) ?
            m_tableHandEye->item(row, 2)->text().toDouble() : 0;

        HECPoint3D robotPoint;
        robotPoint.x = m_tableHandEye->item(row, 6) ?
            m_tableHandEye->item(row, 6)->text().toDouble() : 0;
        robotPoint.y = m_tableHandEye->item(row, 7) ?
            m_tableHandEye->item(row, 7)->text().toDouble() : 0;
        robotPoint.z = m_tableHandEye->item(row, 8) ?
            m_tableHandEye->item(row, 8)->text().toDouble() : 0;

        eyePoints.push_back(eyePoint);
        robotPoints.push_back(robotPoint);
    }
}

void CalibDataWidget::getEyeInHandData(std::vector<HECEyeInHandData>& calibData) const
{
    calibData.clear();

    // 创建标定实例用于欧拉角转换
    IHandEyeCalib* calib = CreateHandEyeCalibInstance();
    if (!calib) {
        return;
    }

    // 获取用户选择的欧拉角顺序
    HECEulerOrder eulerOrder = getEulerOrder();

    int rowCount = m_tableHandEye->rowCount();

    for (int row = 0; row < rowCount; ++row) {
        HECEyeInHandData data;

        // 获取末端位姿（Eye-In-Hand模式下，Robot数据在前6列）
        double endX = m_tableHandEye->item(row, 0) ?
            m_tableHandEye->item(row, 0)->text().toDouble() : 0;
        double endY = m_tableHandEye->item(row, 1) ?
            m_tableHandEye->item(row, 1)->text().toDouble() : 0;
        double endZ = m_tableHandEye->item(row, 2) ?
            m_tableHandEye->item(row, 2)->text().toDouble() : 0;
        double endRoll = m_tableHandEye->item(row, 3) ?
            m_tableHandEye->item(row, 3)->text().toDouble() : 0;
        double endPitch = m_tableHandEye->item(row, 4) ?
            m_tableHandEye->item(row, 4)->text().toDouble() : 0;
        double endYaw = m_tableHandEye->item(row, 5) ?
            m_tableHandEye->item(row, 5)->text().toDouble() : 0;

        // 使用标定接口统一转换欧拉角到旋转矩阵
        HECEulerAngles endAngles(endRoll, endPitch, endYaw);
        HECRotationMatrix R_end;
        calib->EulerToRotationMatrix(endAngles, eulerOrder, R_end);
        HECTranslationVector T_end(endX, endY, endZ);
        data.endPose = HECHomogeneousMatrix(R_end, T_end);

        // 获取相机观测点（Eye-In-Hand模式下，Cam数据在后6列）
        data.targetInCamera.x = m_tableHandEye->item(row, 6) ?
            m_tableHandEye->item(row, 6)->text().toDouble() : 0;
        data.targetInCamera.y = m_tableHandEye->item(row, 7) ?
            m_tableHandEye->item(row, 7)->text().toDouble() : 0;
        data.targetInCamera.z = m_tableHandEye->item(row, 8) ?
            m_tableHandEye->item(row, 8)->text().toDouble() : 0;

        calibData.push_back(data);
    }

    DestroyHandEyeCalibInstance(calib);
}

void CalibDataWidget::getEyeToHandPoseData(std::vector<HECEyeToHandData>& calibData) const
{
    calibData.clear();

    // 创建标定实例用于欧拉角转换
    IHandEyeCalib* calib = CreateHandEyeCalibInstance();
    if (!calib) {
        return;
    }

    // 获取用户选择的欧拉角顺序
    HECEulerOrder eulerOrder = getEulerOrder();

    int rowCount = m_tableHandEye->rowCount();

    for (int row = 0; row < rowCount; ++row) {
        HECEyeToHandData data;

        // 获取末端位姿（Eye-To-Hand模式下，Robot数据在后6列）
        double endX = m_tableHandEye->item(row, 6) ?
            m_tableHandEye->item(row, 6)->text().toDouble() : 0;
        double endY = m_tableHandEye->item(row, 7) ?
            m_tableHandEye->item(row, 7)->text().toDouble() : 0;
        double endZ = m_tableHandEye->item(row, 8) ?
            m_tableHandEye->item(row, 8)->text().toDouble() : 0;

        // 机器人姿态是角度，需要转换为弧度
        double endRoll = m_tableHandEye->item(row, 9) ?
            m_tableHandEye->item(row, 9)->text().toDouble() * M_PI / 180.0 : 0;
        double endPitch = m_tableHandEye->item(row, 10) ?
            m_tableHandEye->item(row, 10)->text().toDouble() * M_PI / 180.0 : 0;
        double endYaw = m_tableHandEye->item(row, 11) ?
            m_tableHandEye->item(row, 11)->text().toDouble() * M_PI / 180.0 : 0;

        // 末端位姿和标定板姿态统一使用相同的欧拉角顺序
        HECEulerAngles endAngles(endRoll, endPitch, endYaw);
        HECRotationMatrix R_end;
        calib->EulerToRotationMatrix(endAngles, eulerOrder, R_end);
        HECTranslationVector T_end(endX, endY, endZ);
        data.endPose = HECHomogeneousMatrix(R_end, T_end);

        // 获取标定板在相机坐标系的位置（Eye-To-Hand模式下，Cam数据在前6列）
        double boardX = m_tableHandEye->item(row, 0) ?
            m_tableHandEye->item(row, 0)->text().toDouble() : 0;
        double boardY = m_tableHandEye->item(row, 1) ?
            m_tableHandEye->item(row, 1)->text().toDouble() : 0;
        double boardZ = m_tableHandEye->item(row, 2) ?
            m_tableHandEye->item(row, 2)->text().toDouble() : 0;

        // 获取标定板在相机坐标系的姿态（Rodrigues旋转向量，单位：弧度）
        double boardRoll = m_tableHandEye->item(row, 3) ?
            m_tableHandEye->item(row, 3)->text().toDouble() : 0;
        double boardPitch = m_tableHandEye->item(row, 4) ?
            m_tableHandEye->item(row, 4)->text().toDouble() : 0;
        double boardYaw = m_tableHandEye->item(row, 5) ?
            m_tableHandEye->item(row, 5)->text().toDouble() : 0;

        // 标定板姿态是Rodrigues旋转向量，需要转换为旋转矩阵
        // 旋转向量的模长是旋转角度，方向是旋转轴
        double theta = std::sqrt(boardRoll*boardRoll + boardPitch*boardPitch + boardYaw*boardYaw);
        HECRotationMatrix R_board;

        if (theta < 1e-6) {
            // 旋转角度接近0，使用单位矩阵
            R_board.at(0, 0) = 1; R_board.at(0, 1) = 0; R_board.at(0, 2) = 0;
            R_board.at(1, 0) = 0; R_board.at(1, 1) = 1; R_board.at(1, 2) = 0;
            R_board.at(2, 0) = 0; R_board.at(2, 1) = 0; R_board.at(2, 2) = 1;
        } else {
            // Rodrigues公式：R = I + sin(θ)/θ * K + (1-cos(θ))/θ² * K²
            double rx = boardRoll / theta;
            double ry = boardPitch / theta;
            double rz = boardYaw / theta;

            double c = std::cos(theta);
            double s = std::sin(theta);
            double c1 = 1.0 - c;

            R_board.at(0, 0) = c + rx*rx*c1;
            R_board.at(0, 1) = rx*ry*c1 - rz*s;
            R_board.at(0, 2) = rx*rz*c1 + ry*s;

            R_board.at(1, 0) = ry*rx*c1 + rz*s;
            R_board.at(1, 1) = c + ry*ry*c1;
            R_board.at(1, 2) = ry*rz*c1 - rx*s;

            R_board.at(2, 0) = rz*rx*c1 - ry*s;
            R_board.at(2, 1) = rz*ry*c1 + rx*s;
            R_board.at(2, 2) = c + rz*rz*c1;
        }

        HECTranslationVector T_board(boardX, boardY, boardZ);
        data.boardInCamera = HECHomogeneousMatrix(R_board, T_board);

        calibData.push_back(data);
    }

    DestroyHandEyeCalibInstance(calib);
}

HECCalibrationType CalibDataWidget::getCalibType() const
{
    return m_cbCalibType->currentIndex() == 0 ?
        HECCalibrationType::EyeToHand : HECCalibrationType::EyeInHand;
}

int CalibDataWidget::getCalibTypeIndex() const
{
    return m_cbCalibType->currentIndex();
}

HECEulerOrder CalibDataWidget::getEulerOrder() const
{
    switch (m_cbEulerOrder->currentIndex()) {
    case 0: return HECEulerOrder::ZYX;
    case 1: return HECEulerOrder::XYZ;
    case 2: return HECEulerOrder::XZY;
    case 3: return HECEulerOrder::YXZ;
    case 4: return HECEulerOrder::YZX;
    case 5: return HECEulerOrder::ZXY;
    default: return HECEulerOrder::ZYX;
    }
}

void CalibDataWidget::clearAll()
{
    m_tableHandEye->setRowCount(0);
    m_tableTCP->setRowCount(0);
}

QWidget* CalibDataWidget::createTCPCalibGroup()
{
    QWidget* group = new QWidget(this);
    QVBoxLayout* layout = new QVBoxLayout(group);
    layout->setContentsMargins(0, 0, 0, 0);

    QHBoxLayout* modeLayout = new QHBoxLayout();
    modeLayout->addWidget(new QLabel("标定模式:", this));
    m_tcpModeCombo = new QComboBox(this);
    m_tcpModeCombo->addItem("位置标定 (3-DOF)");
    m_tcpModeCombo->addItem("完整标定 (6-DOF)");
    connect(m_tcpModeCombo, QOverload<int>::of(&QComboBox::currentIndexChanged),
            this, &CalibDataWidget::onTCPModeChanged);
    modeLayout->addWidget(m_tcpModeCombo);

    modeLayout->addWidget(new QLabel("欧拉角顺序:", this));
    m_tcpEulerOrderCombo = new QComboBox(this);
    m_tcpEulerOrderCombo->addItem("XYZ", static_cast<int>(HECEulerOrder::XYZ));
    m_tcpEulerOrderCombo->addItem("XZY", static_cast<int>(HECEulerOrder::XZY));
    m_tcpEulerOrderCombo->addItem("YXZ", static_cast<int>(HECEulerOrder::YXZ));
    m_tcpEulerOrderCombo->addItem("YZX", static_cast<int>(HECEulerOrder::YZX));
    m_tcpEulerOrderCombo->addItem("ZXY", static_cast<int>(HECEulerOrder::ZXY));
    m_tcpEulerOrderCombo->addItem("ZYX (常用)", static_cast<int>(HECEulerOrder::ZYX));
    m_tcpEulerOrderCombo->setCurrentIndex(5);
    modeLayout->addWidget(m_tcpEulerOrderCombo);
    modeLayout->addStretch();
    layout->addLayout(modeLayout);

    m_tableTCP = new QTableWidget(this);
    m_tableTCP->setColumnCount(6);
    m_tableTCP->setHorizontalHeaderLabels({"X", "Y", "Z", "Roll(°)", "Pitch(°)", "Yaw(°)"});
    m_tableTCP->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch);
    m_tableTCP->setSelectionBehavior(QAbstractItemView::SelectRows);
    layout->addWidget(m_tableTCP, 1);

    QHBoxLayout* btnLayout = new QHBoxLayout();
    m_tcpAddRowBtn = new QPushButton("添加行", this);
    m_tcpRemoveRowBtn = new QPushButton("删除行", this);
    m_btnTCPCalib = new QPushButton("TCP 标定", this);
    connect(m_tcpAddRowBtn, &QPushButton::clicked, this, &CalibDataWidget::onTCPAddRow);
    connect(m_tcpRemoveRowBtn, &QPushButton::clicked, this, &CalibDataWidget::onTCPRemoveRow);
    connect(m_btnTCPCalib, &QPushButton::clicked, this, &CalibDataWidget::requestTCPCalib);
    btnLayout->addWidget(m_tcpAddRowBtn);
    btnLayout->addWidget(m_tcpRemoveRowBtn);
    btnLayout->addWidget(m_btnTCPCalib);
    btnLayout->addStretch();
    layout->addLayout(btnLayout);

    QGroupBox* inputGroup = new QGroupBox("数据输入", group);
    QGridLayout* inputLayout = new QGridLayout(inputGroup);
    inputLayout->setHorizontalSpacing(2);
    inputLayout->setVerticalSpacing(4);
    inputLayout->setContentsMargins(4, 4, 4, 4);
    inputLayout->setColumnStretch(0, 0);
    inputLayout->setColumnStretch(1, 1);
    inputLayout->setColumnStretch(2, 0);
    inputLayout->setColumnStretch(3, 1);
    inputLayout->setColumnStretch(4, 0);
    inputLayout->setColumnStretch(5, 1);

    auto createSpinBox = [this]() {
        QDoubleSpinBox* sb = new QDoubleSpinBox(this);
        sb->setRange(-10000, 10000);
        sb->setDecimals(3);
        return sb;
    };

    inputLayout->addWidget(new QLabel("X:", this), 0, 0);
    m_inputTcpX = createSpinBox();
    inputLayout->addWidget(m_inputTcpX, 0, 1);
    inputLayout->addWidget(new QLabel("Y:", this), 0, 2);
    m_inputTcpY = createSpinBox();
    inputLayout->addWidget(m_inputTcpY, 0, 3);
    inputLayout->addWidget(new QLabel("Z:", this), 0, 4);
    m_inputTcpZ = createSpinBox();
    inputLayout->addWidget(m_inputTcpZ, 0, 5);

    inputLayout->addWidget(new QLabel("Roll(°):", this), 1, 0);
    m_inputTcpRx = createSpinBox();
    inputLayout->addWidget(m_inputTcpRx, 1, 1);
    inputLayout->addWidget(new QLabel("Pitch(°):", this), 1, 2);
    m_inputTcpRy = createSpinBox();
    inputLayout->addWidget(m_inputTcpRy, 1, 3);
    inputLayout->addWidget(new QLabel("Yaw(°):", this), 1, 4);
    m_inputTcpRz = createSpinBox();
    inputLayout->addWidget(m_inputTcpRz, 1, 5);

    m_btnTcpAddInput = new QPushButton("添加到表格", this);
    connect(m_btnTcpAddInput, &QPushButton::clicked, this, [this]() {
        int row = m_tableTCP->rowCount();
        m_tableTCP->insertRow(row);
        m_tableTCP->setItem(row, 0, new QTableWidgetItem(QString::number(m_inputTcpX->value(), 'f', 3)));
        m_tableTCP->setItem(row, 1, new QTableWidgetItem(QString::number(m_inputTcpY->value(), 'f', 3)));
        m_tableTCP->setItem(row, 2, new QTableWidgetItem(QString::number(m_inputTcpZ->value(), 'f', 3)));
        m_tableTCP->setItem(row, 3, new QTableWidgetItem(QString::number(m_inputTcpRx->value(), 'f', 3)));
        m_tableTCP->setItem(row, 4, new QTableWidgetItem(QString::number(m_inputTcpRy->value(), 'f', 3)));
        m_tableTCP->setItem(row, 5, new QTableWidgetItem(QString::number(m_inputTcpRz->value(), 'f', 3)));
        m_tableTCP->scrollToBottom();
    });
    inputLayout->addWidget(m_btnTcpAddInput, 2, 0, 1, 6);

    layout->addWidget(inputGroup);

    m_tcpOrientationGroup = new QGroupBox("6-DOF 参数", group);
    QGridLayout* oriLayout = new QGridLayout(m_tcpOrientationGroup);

    oriLayout->addWidget(new QLabel("参考位姿索引:", this), 0, 0);
    m_tcpRefPoseIndex = new QSpinBox(this);
    m_tcpRefPoseIndex->setRange(0, 999);
    oriLayout->addWidget(m_tcpRefPoseIndex, 0, 1);

    oriLayout->addWidget(new QLabel("期望世界朝向 Rx(°):", this), 1, 0);
    m_tcpWorldRx = new QDoubleSpinBox(this);
    m_tcpWorldRx->setRange(-360, 360);
    m_tcpWorldRx->setDecimals(2);
    oriLayout->addWidget(m_tcpWorldRx, 1, 1);

    oriLayout->addWidget(new QLabel("Ry(°):", this), 1, 2);
    m_tcpWorldRy = new QDoubleSpinBox(this);
    m_tcpWorldRy->setRange(-360, 360);
    m_tcpWorldRy->setDecimals(2);
    oriLayout->addWidget(m_tcpWorldRy, 1, 3);

    oriLayout->addWidget(new QLabel("Rz(°):", this), 1, 4);
    m_tcpWorldRz = new QDoubleSpinBox(this);
    m_tcpWorldRz->setRange(-360, 360);
    m_tcpWorldRz->setDecimals(2);
    oriLayout->addWidget(m_tcpWorldRz, 1, 5);

    m_tcpOrientationGroup->setVisible(false);
    layout->addWidget(m_tcpOrientationGroup);

    return group;
}

void CalibDataWidget::onTCPModeChanged(int index)
{
    m_tcpOrientationGroup->setVisible(index == 1);
}

void CalibDataWidget::onTCPAddRow()
{
    int row = m_tableTCP->rowCount();
    m_tableTCP->insertRow(row);
    for (int col = 0; col < 6; ++col) {
        m_tableTCP->setItem(row, col, new QTableWidgetItem("0"));
    }
    m_tableTCP->scrollToBottom();
}

void CalibDataWidget::onTCPRemoveRow()
{
    int row = m_tableTCP->currentRow();
    if (row >= 0) {
        m_tableTCP->removeRow(row);
    }
}

HECTCPCalibData CalibDataWidget::getTCPCalibData() const
{
    HECTCPCalibData data;

    data.mode = (m_tcpModeCombo->currentIndex() == 0) ?
        HECTCPCalibMode::PositionOnly : HECTCPCalibMode::Full6DOF;

    HECEulerOrder eulerOrder = static_cast<HECEulerOrder>(
        m_tcpEulerOrderCombo->currentData().toInt());

    for (int row = 0; row < m_tableTCP->rowCount(); ++row) {
        HECTCPCalibPose pose;
        pose.x  = m_tableTCP->item(row, 0) ? m_tableTCP->item(row, 0)->text().toDouble() : 0;
        pose.y  = m_tableTCP->item(row, 1) ? m_tableTCP->item(row, 1)->text().toDouble() : 0;
        pose.z  = m_tableTCP->item(row, 2) ? m_tableTCP->item(row, 2)->text().toDouble() : 0;
        pose.rx = m_tableTCP->item(row, 3) ? m_tableTCP->item(row, 3)->text().toDouble() : 0;
        pose.ry = m_tableTCP->item(row, 4) ? m_tableTCP->item(row, 4)->text().toDouble() : 0;
        pose.rz = m_tableTCP->item(row, 5) ? m_tableTCP->item(row, 5)->text().toDouble() : 0;
        pose.eulerOrder = eulerOrder;
        data.poses.push_back(pose);
    }

    data.referencePoseIndex = m_tcpRefPoseIndex->value();
    data.worldRx = m_tcpWorldRx->value();
    data.worldRy = m_tcpWorldRy->value();
    data.worldRz = m_tcpWorldRz->value();
    data.worldEulerOrder = eulerOrder;

    return data;
}

void CalibDataWidget::setRobotInput(double x, double y, double z,
                                     double rx, double ry, double rz)
{
    int mode = m_cbCalibType->currentIndex();
    
    if (mode == 0) {
        m_inputRobotX->setValue(x);
        m_inputRobotY->setValue(y);
        m_inputRobotZ->setValue(z);
        m_inputRobotRoll->setValue(rx);
        m_inputRobotPitch->setValue(ry);
        m_inputRobotYaw->setValue(rz);
    } else if (mode == 1) {
        m_inputRobotX->setValue(x);
        m_inputRobotY->setValue(y);
        m_inputRobotZ->setValue(z);
        m_inputRobotRoll->setValue(rx);
        m_inputRobotPitch->setValue(ry);
        m_inputRobotYaw->setValue(rz);
    } else if (mode == 2) {
        m_inputTcpX->setValue(x);
        m_inputTcpY->setValue(y);
        m_inputTcpZ->setValue(z);
        m_inputTcpRx->setValue(rx);
        m_inputTcpRy->setValue(ry);
        m_inputTcpRz->setValue(rz);
    }
}

void CalibDataWidget::setCameraInput(double x, double y, double z,
                                      double rx, double ry, double rz)
{
    m_inputCamX->setValue(x);
    m_inputCamY->setValue(y);
    m_inputCamZ->setValue(z);
    m_inputCamRoll->setValue(rx);
    m_inputCamPitch->setValue(ry);
    m_inputCamYaw->setValue(rz);
}

void CalibDataWidget::saveCalibData(const QString& filePath) const
{
    QSettings ini(filePath, QSettings::IniFormat);
    ini.setIniCodec("UTF-8");

    int mode = m_cbCalibType->currentIndex();

    ini.beginGroup("CommInfo");
    ini.setValue("eCalibType", mode);
    ini.setValue("sCalibTime", QDateTime::currentDateTime().toString("yyyy-MM-dd-HH-mm-ss"));

    if (mode == 0) {
        int count = m_tableHandEye->rowCount();
        ini.setValue("nPointCount", count);
        ini.endGroup();

        for (int row = 0; row < count; ++row) {
            ini.beginGroup(QString("CalibPointInfo_%1").arg(row));
            ini.setValue("dCamX",   m_tableHandEye->item(row, 0) ? m_tableHandEye->item(row, 0)->text() : "0");
            ini.setValue("dCamY",   m_tableHandEye->item(row, 1) ? m_tableHandEye->item(row, 1)->text() : "0");
            ini.setValue("dCamZ",   m_tableHandEye->item(row, 2) ? m_tableHandEye->item(row, 2)->text() : "0");
            ini.setValue("dCamRoll",   m_tableHandEye->item(row, 3) ? m_tableHandEye->item(row, 3)->text() : "0");
            ini.setValue("dCamPitch",   m_tableHandEye->item(row, 4) ? m_tableHandEye->item(row, 4)->text() : "0");
            ini.setValue("dCamYaw",   m_tableHandEye->item(row, 5) ? m_tableHandEye->item(row, 5)->text() : "0");
            ini.setValue("dRobotX", m_tableHandEye->item(row, 6) ? m_tableHandEye->item(row, 6)->text() : "0");
            ini.setValue("dRobotY", m_tableHandEye->item(row, 7) ? m_tableHandEye->item(row, 7)->text() : "0");
            ini.setValue("dRobotZ", m_tableHandEye->item(row, 8) ? m_tableHandEye->item(row, 8)->text() : "0");
            ini.setValue("dRobotRoll", m_tableHandEye->item(row, 9) ? m_tableHandEye->item(row, 9)->text() : "0");
            ini.setValue("dRobotPitch", m_tableHandEye->item(row, 10) ? m_tableHandEye->item(row, 10)->text() : "0");
            ini.setValue("dRobotYaw", m_tableHandEye->item(row, 11) ? m_tableHandEye->item(row, 11)->text() : "0");
            ini.endGroup();
        }
    } else if (mode == 1) {
        int count = m_tableHandEye->rowCount();
        ini.setValue("nPointCount", count);
        ini.endGroup();

        for (int row = 0; row < count; ++row) {
            ini.beginGroup(QString("CalibPointInfo_%1").arg(row));
            ini.setValue("dEndX",     m_tableHandEye->item(row, 0) ? m_tableHandEye->item(row, 0)->text() : "0");
            ini.setValue("dEndY",     m_tableHandEye->item(row, 1) ? m_tableHandEye->item(row, 1)->text() : "0");
            ini.setValue("dEndZ",     m_tableHandEye->item(row, 2) ? m_tableHandEye->item(row, 2)->text() : "0");
            ini.setValue("dEndRoll",  m_tableHandEye->item(row, 3) ? m_tableHandEye->item(row, 3)->text() : "0");
            ini.setValue("dEndPitch", m_tableHandEye->item(row, 4) ? m_tableHandEye->item(row, 4)->text() : "0");
            ini.setValue("dEndYaw",   m_tableHandEye->item(row, 5) ? m_tableHandEye->item(row, 5)->text() : "0");
            ini.setValue("dCamX",     m_tableHandEye->item(row, 6) ? m_tableHandEye->item(row, 6)->text() : "0");
            ini.setValue("dCamY",     m_tableHandEye->item(row, 7) ? m_tableHandEye->item(row, 7)->text() : "0");
            ini.setValue("dCamZ",     m_tableHandEye->item(row, 8) ? m_tableHandEye->item(row, 8)->text() : "0");
            ini.setValue("dCamRoll",  m_tableHandEye->item(row, 9) ? m_tableHandEye->item(row, 9)->text() : "0");
            ini.setValue("dCamPitch", m_tableHandEye->item(row, 10) ? m_tableHandEye->item(row, 10)->text() : "0");
            ini.setValue("dCamYaw",   m_tableHandEye->item(row, 11) ? m_tableHandEye->item(row, 11)->text() : "0");
            ini.endGroup();
        }
    } else {
        int count = m_tableTCP->rowCount();
        ini.setValue("nPoseCount", count);
        ini.setValue("eCalibMode", m_tcpModeCombo->currentIndex());
        ini.setValue("eEulerOrder", m_tcpEulerOrderCombo->currentIndex());
        ini.setValue("nRefPoseIndex", m_tcpRefPoseIndex->value());
        ini.setValue("dWorldRx", m_tcpWorldRx->value());
        ini.setValue("dWorldRy", m_tcpWorldRy->value());
        ini.setValue("dWorldRz", m_tcpWorldRz->value());
        ini.endGroup();

        for (int row = 0; row < count; ++row) {
            ini.beginGroup(QString("CalibPoseInfo_%1").arg(row));
            ini.setValue("dX",  m_tableTCP->item(row, 0) ? m_tableTCP->item(row, 0)->text() : "0");
            ini.setValue("dY",  m_tableTCP->item(row, 1) ? m_tableTCP->item(row, 1)->text() : "0");
            ini.setValue("dZ",  m_tableTCP->item(row, 2) ? m_tableTCP->item(row, 2)->text() : "0");
            ini.setValue("dRx", m_tableTCP->item(row, 3) ? m_tableTCP->item(row, 3)->text() : "0");
            ini.setValue("dRy", m_tableTCP->item(row, 4) ? m_tableTCP->item(row, 4)->text() : "0");
            ini.setValue("dRz", m_tableTCP->item(row, 5) ? m_tableTCP->item(row, 5)->text() : "0");
            ini.endGroup();
        }
    }
}

void CalibDataWidget::loadCalibData(const QString& filePath)
{
    QSettings ini(filePath, QSettings::IniFormat);
    ini.setIniCodec("UTF-8");

    ini.beginGroup("CommInfo");
    int calibType = ini.value("eCalibType", -1).toInt();

    // 读取欧拉角顺序
    QString axisOrder = ini.value("eRobotPosAxisOrder", "RxRyRz").toString();

    // 映射到UI的欧拉角顺序选择
    if (axisOrder == "RxRyRz") {
        m_cbEulerOrder->setCurrentIndex(1); // XYZ
    } else if (axisOrder == "RxRzRy") {
        m_cbEulerOrder->setCurrentIndex(2); // XZY
    } else if (axisOrder == "RyRxRz") {
        m_cbEulerOrder->setCurrentIndex(3); // YXZ
    } else if (axisOrder == "RyRzRx") {
        m_cbEulerOrder->setCurrentIndex(4); // YZX
    } else if (axisOrder == "RzRxRy") {
        m_cbEulerOrder->setCurrentIndex(5); // ZXY
    } else {
        m_cbEulerOrder->setCurrentIndex(0); // ZYX (默认)
    }

    if (calibType == 0) {
        int count = ini.value("nPointCount", 0).toInt();
        int itemCount = ini.value("nCalibItemNum", 0).toInt();
        ini.endGroup();

        m_cbCalibType->setCurrentIndex(0);

        if (itemCount > 0) {
            m_tableHandEye->setRowCount(0);
            m_boardPoseData.clear();

            for (int i = 0; i < itemCount; ++i) {
                ini.beginGroup(QString("pCalibItemList_%1").arg(i));
                int row = m_tableHandEye->rowCount();
                m_tableHandEye->insertRow(row);

                m_tableHandEye->setItem(row, 0, new QTableWidgetItem(ini.value("dExtrinsic_0", "0").toString()));
                m_tableHandEye->setItem(row, 1, new QTableWidgetItem(ini.value("dExtrinsic_1", "0").toString()));
                m_tableHandEye->setItem(row, 2, new QTableWidgetItem(ini.value("dExtrinsic_2", "0").toString()));

                double camRoll = ini.value("dExtrinsic_3", 0).toDouble();
                double camPitch = ini.value("dExtrinsic_4", 0).toDouble();
                double camYaw = ini.value("dExtrinsic_5", 0).toDouble();
                m_tableHandEye->setItem(row, 3, new QTableWidgetItem(QString::number(camRoll, 'f', 6)));
                m_tableHandEye->setItem(row, 4, new QTableWidgetItem(QString::number(camPitch, 'f', 6)));
                m_tableHandEye->setItem(row, 5, new QTableWidgetItem(QString::number(camYaw, 'f', 6)));

                m_tableHandEye->setItem(row, 6, new QTableWidgetItem(ini.value("sRobotPos_x", "0").toString()));
                m_tableHandEye->setItem(row, 7, new QTableWidgetItem(ini.value("sRobotPos_y", "0").toString()));
                m_tableHandEye->setItem(row, 8, new QTableWidgetItem(ini.value("sRobotPos_z", "0").toString()));

                // 机器人姿态保持角度（不转换）
                double robotRoll = ini.value("sRobotPos_roll", 0).toDouble();
                double robotPitch = ini.value("sRobotPos_pitch", 0).toDouble();
                double robotYaw = ini.value("sRobotPos_yaw", 0).toDouble();
                m_tableHandEye->setItem(row, 9, new QTableWidgetItem(QString::number(robotRoll, 'f', 6)));
                m_tableHandEye->setItem(row, 10, new QTableWidgetItem(QString::number(robotPitch, 'f', 6)));
                m_tableHandEye->setItem(row, 11, new QTableWidgetItem(QString::number(robotYaw, 'f', 6)));

                m_boardPoseData.push_back({camRoll, camPitch, camYaw});

                ini.endGroup();
            }
        } else if (count > 0) {
            m_tableHandEye->setRowCount(0);
            for (int i = 0; i < count; ++i) {
                ini.beginGroup(QString("CalibPointInfo_%1").arg(i));
                int row = m_tableHandEye->rowCount();
                m_tableHandEye->insertRow(row);
                m_tableHandEye->setItem(row, 0, new QTableWidgetItem(ini.value("dCamX", "0").toString()));
                m_tableHandEye->setItem(row, 1, new QTableWidgetItem(ini.value("dCamY", "0").toString()));
                m_tableHandEye->setItem(row, 2, new QTableWidgetItem(ini.value("dCamZ", "0").toString()));
                m_tableHandEye->setItem(row, 3, new QTableWidgetItem(ini.value("dCamRoll", "0").toString()));
                m_tableHandEye->setItem(row, 4, new QTableWidgetItem(ini.value("dCamPitch", "0").toString()));
                m_tableHandEye->setItem(row, 5, new QTableWidgetItem(ini.value("dCamYaw", "0").toString()));
                m_tableHandEye->setItem(row, 6, new QTableWidgetItem(ini.value("dRobotX", "0").toString()));
                m_tableHandEye->setItem(row, 7, new QTableWidgetItem(ini.value("dRobotY", "0").toString()));
                m_tableHandEye->setItem(row, 8, new QTableWidgetItem(ini.value("dRobotZ", "0").toString()));
                m_tableHandEye->setItem(row, 9, new QTableWidgetItem(ini.value("dRobotRoll", "0").toString()));
                m_tableHandEye->setItem(row, 10, new QTableWidgetItem(ini.value("dRobotPitch", "0").toString()));
                m_tableHandEye->setItem(row, 11, new QTableWidgetItem(ini.value("dRobotYaw", "0").toString()));
                ini.endGroup();
            }
        }
    } else if (calibType == 1) {
        int count = ini.value("nPointCount", 0).toInt();
        ini.endGroup();

        m_cbCalibType->setCurrentIndex(1);
        m_tableHandEye->setRowCount(0);
        for (int i = 0; i < count; ++i) {
            ini.beginGroup(QString("CalibPointInfo_%1").arg(i));
            int row = m_tableHandEye->rowCount();
            m_tableHandEye->insertRow(row);
            m_tableHandEye->setItem(row, 0, new QTableWidgetItem(ini.value("dEndX", "0").toString()));
            m_tableHandEye->setItem(row, 1, new QTableWidgetItem(ini.value("dEndY", "0").toString()));
            m_tableHandEye->setItem(row, 2, new QTableWidgetItem(ini.value("dEndZ", "0").toString()));
            m_tableHandEye->setItem(row, 3, new QTableWidgetItem(ini.value("dEndRoll", "0").toString()));
            m_tableHandEye->setItem(row, 4, new QTableWidgetItem(ini.value("dEndPitch", "0").toString()));
            m_tableHandEye->setItem(row, 5, new QTableWidgetItem(ini.value("dEndYaw", "0").toString()));
            m_tableHandEye->setItem(row, 6, new QTableWidgetItem(ini.value("dCamX", "0").toString()));
            m_tableHandEye->setItem(row, 7, new QTableWidgetItem(ini.value("dCamY", "0").toString()));
            m_tableHandEye->setItem(row, 8, new QTableWidgetItem(ini.value("dCamZ", "0").toString()));
            m_tableHandEye->setItem(row, 9, new QTableWidgetItem(ini.value("dCamRoll", "0").toString()));
            m_tableHandEye->setItem(row, 10, new QTableWidgetItem(ini.value("dCamPitch", "0").toString()));
            m_tableHandEye->setItem(row, 11, new QTableWidgetItem(ini.value("dCamYaw", "0").toString()));
            ini.endGroup();
        }
    } else if (calibType == 2) {
        int count = ini.value("nPoseCount", 0).toInt();
        m_tcpModeCombo->setCurrentIndex(ini.value("eCalibMode", 0).toInt());
        m_tcpEulerOrderCombo->setCurrentIndex(ini.value("eEulerOrder", 5).toInt());
        m_tcpRefPoseIndex->setValue(ini.value("nRefPoseIndex", 0).toInt());
        m_tcpWorldRx->setValue(ini.value("dWorldRx", 0).toDouble());
        m_tcpWorldRy->setValue(ini.value("dWorldRy", 0).toDouble());
        m_tcpWorldRz->setValue(ini.value("dWorldRz", 0).toDouble());
        ini.endGroup();

        m_cbCalibType->setCurrentIndex(2);
        m_tableTCP->setRowCount(0);
        for (int i = 0; i < count; ++i) {
            ini.beginGroup(QString("CalibPoseInfo_%1").arg(i));
            int row = m_tableTCP->rowCount();
            m_tableTCP->insertRow(row);
            m_tableTCP->setItem(row, 0, new QTableWidgetItem(ini.value("dX", "0").toString()));
            m_tableTCP->setItem(row, 1, new QTableWidgetItem(ini.value("dY", "0").toString()));
            m_tableTCP->setItem(row, 2, new QTableWidgetItem(ini.value("dZ", "0").toString()));
            m_tableTCP->setItem(row, 3, new QTableWidgetItem(ini.value("dRx", "0").toString()));
            m_tableTCP->setItem(row, 4, new QTableWidgetItem(ini.value("dRy", "0").toString()));
            m_tableTCP->setItem(row, 5, new QTableWidgetItem(ini.value("dRz", "0").toString()));
            ini.endGroup();
        }
    } else {
        ini.endGroup();
    }
}