GrabBag/Tools/CalibView/Src/CalibDataWidget.cpp

#include "CalibDataWidget.h"

#include <QVBoxLayout>
#include <QHBoxLayout>
#include <QGridLayout>
#include <QHeaderView>
#include <QLabel>

CalibDataWidget::CalibDataWidget(QWidget* parent)
    : QWidget(parent)
    , m_cbCalibType(nullptr)
    , m_tableEyeToHand(nullptr)
    , m_groupEyeToHand(nullptr)
    , m_tableEyeInHand(nullptr)
    , m_groupEyeInHand(nullptr)
    , m_sbTransformX(nullptr)
    , m_sbTransformY(nullptr)
    , m_sbTransformZ(nullptr)
    , m_sbRoll(nullptr)
    , m_sbPitch(nullptr)
    , m_sbYaw(nullptr)
    , m_cbEulerOrder(nullptr)
    , m_btnAddRow(nullptr)
    , m_btnDeleteRow(nullptr)
{
    setupUI();
}

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 (眼在手上)");
    connect(m_cbCalibType, QOverload<int>::of(&QComboBox::currentIndexChanged),
            this, &CalibDataWidget::onCalibTypeChanged);
    modeLayout->addWidget(lblMode);
    modeLayout->addWidget(m_cbCalibType);
    modeLayout->addStretch();
    mainLayout->addLayout(modeLayout);

    // Eye-To-Hand 数据组
    m_groupEyeToHand = createEyeToHandGroup();
    mainLayout->addWidget(m_groupEyeToHand);

    // Eye-In-Hand 数据组
    m_groupEyeInHand = createEyeInHandGroup();
    mainLayout->addWidget(m_groupEyeInHand);
    m_groupEyeInHand->setVisible(false);

    // 按钮行
    QHBoxLayout* btnLayout = new QHBoxLayout();
    m_btnAddRow = new QPushButton("添加行", this);
    m_btnDeleteRow = new QPushButton("删除行", this);
    connect(m_btnAddRow, &QPushButton::clicked, this, &CalibDataWidget::onAddRow);
    connect(m_btnDeleteRow, &QPushButton::clicked, this, &CalibDataWidget::onDeleteRow);
    btnLayout->addWidget(m_btnAddRow);
    btnLayout->addWidget(m_btnDeleteRow);
    btnLayout->addStretch();
    mainLayout->addLayout(btnLayout);

    // 变换测试组
    mainLayout->addWidget(createTransformGroup());

    // 欧拉角测试组
    mainLayout->addWidget(createEulerGroup());

    mainLayout->addStretch();
}

QGroupBox* CalibDataWidget::createEyeToHandGroup()
{
    QGroupBox* group = new QGroupBox("Eye-To-Hand 标定数据", this);
    QVBoxLayout* layout = new QVBoxLayout(group);

    m_tableEyeToHand = new QTableWidget(this);
    m_tableEyeToHand->setColumnCount(6);
    m_tableEyeToHand->setHorizontalHeaderLabels({
        "Eye X", "Eye Y", "Eye Z", "Robot X", "Robot Y", "Robot Z"
    });
    m_tableEyeToHand->horizontalHeader()->setStretchLastSection(true);
    m_tableEyeToHand->setSelectionBehavior(QAbstractItemView::SelectRows);

    layout->addWidget(m_tableEyeToHand);
    return group;
}

QGroupBox* CalibDataWidget::createEyeInHandGroup()
{
    QGroupBox* group = new QGroupBox("Eye-In-Hand 标定数据", this);
    QVBoxLayout* layout = new QVBoxLayout(group);

    m_tableEyeInHand = new QTableWidget(this);
    m_tableEyeInHand->setColumnCount(9);
    m_tableEyeInHand->setHorizontalHeaderLabels({
        "End X", "End Y", "End Z", "End Roll", "End Pitch", "End Yaw",
        "Cam X", "Cam Y", "Cam Z"
    });
    m_tableEyeInHand->horizontalHeader()->setStretchLastSection(true);
    m_tableEyeInHand->setSelectionBehavior(QAbstractItemView::SelectRows);

    layout->addWidget(m_tableEyeInHand);
    return group;
}

QGroupBox* CalibDataWidget::createTransformGroup()
{
    QGroupBox* group = new QGroupBox("坐标变换测试", this);
    QGridLayout* layout = new QGridLayout(group);

    layout->addWidget(new QLabel("X:", this), 0, 0);
    m_sbTransformX = new QDoubleSpinBox(this);
    m_sbTransformX->setRange(-10000, 10000);
    m_sbTransformX->setDecimals(3);
    layout->addWidget(m_sbTransformX, 0, 1);

    layout->addWidget(new QLabel("Y:", this), 0, 2);
    m_sbTransformY = new QDoubleSpinBox(this);
    m_sbTransformY->setRange(-10000, 10000);
    m_sbTransformY->setDecimals(3);
    layout->addWidget(m_sbTransformY, 0, 3);

    layout->addWidget(new QLabel("Z:", this), 0, 4);
    m_sbTransformZ = new QDoubleSpinBox(this);
    m_sbTransformZ->setRange(-10000, 10000);
    m_sbTransformZ->setDecimals(3);
    layout->addWidget(m_sbTransformZ, 0, 5);

    return group;
}

QGroupBox* CalibDataWidget::createEulerGroup()
{
    QGroupBox* group = new QGroupBox("欧拉角转换测试", this);
    QGridLayout* layout = new QGridLayout(group);

    layout->addWidget(new QLabel("Roll (°):", this), 0, 0);
    m_sbRoll = new QDoubleSpinBox(this);
    m_sbRoll->setRange(-180, 180);
    m_sbRoll->setDecimals(2);
    layout->addWidget(m_sbRoll, 0, 1);

    layout->addWidget(new QLabel("Pitch (°):", this), 0, 2);
    m_sbPitch = new QDoubleSpinBox(this);
    m_sbPitch->setRange(-180, 180);
    m_sbPitch->setDecimals(2);
    layout->addWidget(m_sbPitch, 0, 3);

    layout->addWidget(new QLabel("Yaw (°):", this), 0, 4);
    m_sbYaw = new QDoubleSpinBox(this);
    m_sbYaw->setRange(-180, 180);
    m_sbYaw->setDecimals(2);
    layout->addWidget(m_sbYaw, 0, 5);

    layout->addWidget(new QLabel("旋转顺序:", this), 1, 0);
    m_cbEulerOrder = new QComboBox(this);
    m_cbEulerOrder->addItem("XYZ", static_cast<int>(HECEulerOrder::XYZ));
    m_cbEulerOrder->addItem("XZY", static_cast<int>(HECEulerOrder::XZY));
    m_cbEulerOrder->addItem("YXZ", static_cast<int>(HECEulerOrder::YXZ));
    m_cbEulerOrder->addItem("YZX", static_cast<int>(HECEulerOrder::YZX));
    m_cbEulerOrder->addItem("ZXY", static_cast<int>(HECEulerOrder::ZXY));
    m_cbEulerOrder->addItem("ZYX (常用)", static_cast<int>(HECEulerOrder::ZYX));
    m_cbEulerOrder->setCurrentIndex(5);  // 默认 ZYX
    layout->addWidget(m_cbEulerOrder, 1, 1, 1, 2);

    return group;
}

void CalibDataWidget::updateTableVisibility()
{
    bool isEyeToHand = (m_cbCalibType->currentIndex() == 0);
    m_groupEyeToHand->setVisible(isEyeToHand);
    m_groupEyeInHand->setVisible(!isEyeToHand);
}

void CalibDataWidget::onAddRow()
{
    if (m_cbCalibType->currentIndex() == 0) {
        int row = m_tableEyeToHand->rowCount();
        m_tableEyeToHand->insertRow(row);
        for (int col = 0; col < 6; ++col) {
            QTableWidgetItem* item = new QTableWidgetItem("0");
            m_tableEyeToHand->setItem(row, col, item);
        }
    } else {
        int row = m_tableEyeInHand->rowCount();
        m_tableEyeInHand->insertRow(row);
        for (int col = 0; col < 9; ++col) {
            QTableWidgetItem* item = new QTableWidgetItem("0");
            m_tableEyeInHand->setItem(row, col, item);
        }
    }
}

void CalibDataWidget::onDeleteRow()
{
    if (m_cbCalibType->currentIndex() == 0) {
        int row = m_tableEyeToHand->currentRow();
        if (row >= 0) {
            m_tableEyeToHand->removeRow(row);
        }
    } else {
        int row = m_tableEyeInHand->currentRow();
        if (row >= 0) {
            m_tableEyeInHand->removeRow(row);
        }
    }
}

void CalibDataWidget::onCalibTypeChanged(int index)
{
    updateTableVisibility();
    emit calibTypeChanged(index == 0 ? HECCalibrationType::EyeToHand : HECCalibrationType::EyeInHand);
}

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

    for (int row = 0; row < m_tableEyeToHand->rowCount(); ++row) {
        HECPoint3D eyePt, robotPt;

        QTableWidgetItem* item0 = m_tableEyeToHand->item(row, 0);
        QTableWidgetItem* item1 = m_tableEyeToHand->item(row, 1);
        QTableWidgetItem* item2 = m_tableEyeToHand->item(row, 2);
        QTableWidgetItem* item3 = m_tableEyeToHand->item(row, 3);
        QTableWidgetItem* item4 = m_tableEyeToHand->item(row, 4);
        QTableWidgetItem* item5 = m_tableEyeToHand->item(row, 5);

        if (item0 && item1 && item2 && item3 && item4 && item5) {
            eyePt.x = item0->text().toDouble();
            eyePt.y = item1->text().toDouble();
            eyePt.z = item2->text().toDouble();
            robotPt.x = item3->text().toDouble();
            robotPt.y = item4->text().toDouble();
            robotPt.z = item5->text().toDouble();

            eyePoints.push_back(eyePt);
            robotPoints.push_back(robotPt);
        }
    }
}

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

    const double deg2rad = M_PI / 180.0;

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

        // 获取末端位姿
        double endX = m_tableEyeInHand->item(row, 0) ?
            m_tableEyeInHand->item(row, 0)->text().toDouble() : 0;
        double endY = m_tableEyeInHand->item(row, 1) ?
            m_tableEyeInHand->item(row, 1)->text().toDouble() : 0;
        double endZ = m_tableEyeInHand->item(row, 2) ?
            m_tableEyeInHand->item(row, 2)->text().toDouble() : 0;
        double endRoll = m_tableEyeInHand->item(row, 3) ?
            m_tableEyeInHand->item(row, 3)->text().toDouble() * deg2rad : 0;
        double endPitch = m_tableEyeInHand->item(row, 4) ?
            m_tableEyeInHand->item(row, 4)->text().toDouble() * deg2rad : 0;
        double endYaw = m_tableEyeInHand->item(row, 5) ?
            m_tableEyeInHand->item(row, 5)->text().toDouble() * deg2rad : 0;

        // 构建末端位姿矩阵
        HECEulerAngles angles(endRoll, endPitch, endYaw);
        HECRotationMatrix R;

        // 使用 ZYX 顺序构建旋转矩阵
        double cr = cos(endRoll), sr = sin(endRoll);
        double cp = cos(endPitch), sp = sin(endPitch);
        double cy = cos(endYaw), sy = sin(endYaw);

        R.at(0, 0) = cy * cp;
        R.at(0, 1) = cy * sp * sr - sy * cr;
        R.at(0, 2) = cy * sp * cr + sy * sr;
        R.at(1, 0) = sy * cp;
        R.at(1, 1) = sy * sp * sr + cy * cr;
        R.at(1, 2) = sy * sp * cr - cy * sr;
        R.at(2, 0) = -sp;
        R.at(2, 1) = cp * sr;
        R.at(2, 2) = cp * cr;

        HECTranslationVector T(endX, endY, endZ);
        data.endPose = HECHomogeneousMatrix(R, T);

        // 获取相机观测点
        data.targetInCamera.x = m_tableEyeInHand->item(row, 6) ?
            m_tableEyeInHand->item(row, 6)->text().toDouble() : 0;
        data.targetInCamera.y = m_tableEyeInHand->item(row, 7) ?
            m_tableEyeInHand->item(row, 7)->text().toDouble() : 0;
        data.targetInCamera.z = m_tableEyeInHand->item(row, 8) ?
            m_tableEyeInHand->item(row, 8)->text().toDouble() : 0;

        calibData.push_back(data);
    }
}

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

HECEulerOrder CalibDataWidget::getEulerOrder() const
{
    return static_cast<HECEulerOrder>(m_cbEulerOrder->currentData().toInt());
}

void CalibDataWidget::clearAll()
{
    m_tableEyeToHand->setRowCount(0);
    m_tableEyeInHand->setRowCount(0);
    m_sbTransformX->setValue(0);
    m_sbTransformY->setValue(0);
    m_sbTransformZ->setValue(0);
    m_sbRoll->setValue(0);
    m_sbPitch->setValue(0);
    m_sbYaw->setValue(0);
}

void CalibDataWidget::loadTestData()
{
    // 清除现有数据
    m_tableEyeToHand->setRowCount(0);

    // 添加测试数据 (Eye-To-Hand)
    // 模拟相机坐标系和机器人坐标系之间的对应点
    struct TestPoint {
        double ex, ey, ez;  // 相机坐标
        double rx, ry, rz;  // 机器人坐标
    };

    TestPoint testData[] = {
        {100.0, 50.0, 200.0, 500.0, 300.0, 100.0},
        {150.0, 80.0, 210.0, 550.0, 330.0, 110.0},
        {120.0, 60.0, 195.0, 520.0, 310.0, 95.0},
        {180.0, 100.0, 220.0, 580.0, 350.0, 120.0},
        {90.0, 40.0, 190.0, 490.0, 290.0, 90.0},
        {200.0, 120.0, 230.0, 600.0, 370.0, 130.0},
        {110.0, 55.0, 198.0, 510.0, 305.0, 98.0},
        {160.0, 85.0, 215.0, 560.0, 335.0, 115.0},
        {130.0, 65.0, 202.0, 530.0, 315.0, 102.0},
        {170.0, 95.0, 218.0, 570.0, 345.0, 118.0}
    };

    for (const auto& pt : testData) {
        int row = m_tableEyeToHand->rowCount();
        m_tableEyeToHand->insertRow(row);
        m_tableEyeToHand->setItem(row, 0, new QTableWidgetItem(QString::number(pt.ex)));
        m_tableEyeToHand->setItem(row, 1, new QTableWidgetItem(QString::number(pt.ey)));
        m_tableEyeToHand->setItem(row, 2, new QTableWidgetItem(QString::number(pt.ez)));
        m_tableEyeToHand->setItem(row, 3, new QTableWidgetItem(QString::number(pt.rx)));
        m_tableEyeToHand->setItem(row, 4, new QTableWidgetItem(QString::number(pt.ry)));
        m_tableEyeToHand->setItem(row, 5, new QTableWidgetItem(QString::number(pt.rz)));
    }

    // 设置变换测试点
    m_sbTransformX->setValue(140.0);
    m_sbTransformY->setValue(70.0);
    m_sbTransformZ->setValue(205.0);

    // 设置欧拉角测试值
    m_sbRoll->setValue(10.0);
    m_sbPitch->setValue(20.0);
    m_sbYaw->setValue(30.0);
}

void CalibDataWidget::setTransformPoint(const HECPoint3D& point)
{
    m_sbTransformX->setValue(point.x);
    m_sbTransformY->setValue(point.y);
    m_sbTransformZ->setValue(point.z);
}

HECPoint3D CalibDataWidget::getTransformPoint() const
{
    return HECPoint3D(
        m_sbTransformX->value(),
        m_sbTransformY->value(),
        m_sbTransformZ->value()
    );
}

void CalibDataWidget::setEulerAngles(const HECEulerAngles& angles)
{
    double roll, pitch, yaw;
    angles.toDegrees(roll, pitch, yaw);
    m_sbRoll->setValue(roll);
    m_sbPitch->setValue(pitch);
    m_sbYaw->setValue(yaw);
}

HECEulerAngles CalibDataWidget::getEulerAngles() const
{
    return HECEulerAngles::fromDegrees(
        m_sbRoll->value(),
        m_sbPitch->value(),
        m_sbYaw->value()
    );
}