#include "stdafx.h" #include "focusWindow.h" focusWindow::focusWindow(QWidget *parent, ImagerOperationBase* imager) { ui.setupUi(this); //读取配置文件 string HPPACfgFile = getPathofEXE() + "\\HPPA.cfg"; Configfile configfile; configfile.setConfigfilePath(HPPACfgFile); if (!configfile.isConfigfileExist()) configfile.createConfigFile(); configfile.parseConfigfile(); configfile.getPositionRestriction(m_iMaxPos, m_iMinPos); disableBeforeConnect(true); setAttribute(Qt::WA_DeleteOnClose);//设置关闭窗体就调用窗体的析构函数 m_Imager = imager; m_FocusState = 0; m_ctrlFocusMotor = nullptr; m_AutoFocusThread = new QThread(); connect(this->ui.connectMotor_btn, SIGNAL(clicked()), this, SLOT(onConnectMotor())); connect(this->ui.logicZero_btn, SIGNAL(clicked()), this, SLOT(onMove2MotorLogicZero())); connect(this->ui.max_btn, SIGNAL(clicked()), this, SLOT(onMove2MotorMax())); connect(this->ui.add_btn, SIGNAL(clicked()), this, SLOT(onAdd())); connect(this->ui.subtract_btn, SIGNAL(clicked()), this, SLOT(onSubtract())); connect(this->ui.ultrasound_radioButton, SIGNAL(released()), this, SLOT(onUltrasound_radioButton())); connect(this->ui.autoFocus_btn, SIGNAL(clicked()), this, SLOT(onAutoFocus())); connect(this->ui.manualFocus_btn, SIGNAL(clicked()), this, SLOT(onManualFocus())); connect(this->ui.updateCurrentLocation_btn, SIGNAL(clicked()), this, SLOT(onUpdateCurrentLocation())); connect(this->ui.moveto_btn, SIGNAL(clicked()), this, SLOT(onMoveto())); connect(this->ui.rangeMeasurement_btn, SIGNAL(pressed()), this, SLOT(onx_rangeMeasurement())); //查找可用串口，并显示 foreach(const QSerialPortInfo &info, QSerialPortInfo::availablePorts()) { QSerialPort serial; serial.setPort(info); if (serial.open(QIODevice::ReadWrite)) { ui.motorPort_comboBox->addItem(serial.portName()); ui.ultrasoundPort_comboBox->addItem(serial.portName()); serial.close(); } } //设置自动调焦进度条 ui.autoFocusProgress_progressBar->setMinimum(0); ui.autoFocusProgress_progressBar->setMaximum(100); ui.autoFocusProgress_progressBar->reset(); m_dSpeed = 1.0; } focusWindow::~focusWindow() { printf("destroy focusWindow-------------------------\n"); emit StartManualFocusSignal(0);//当用户没有点击停止调焦就关闭窗口 delete m_ctrlFocusMotor; //delete thread1, progressThread; m_motorThread.quit(); m_motorThread.wait(); m_MotionCaptureCoordinatorThread.quit(); m_MotionCaptureCoordinatorThread.wait(); } void focusWindow::disableBeforeConnect(bool disable) { ui.controlMotor_groupBox->setDisabled(disable); ui.autoFocus_btn->setDisabled(disable); } bool test(void *pCaller, int *x, int *y, void **pvdata) { focusWindow *p = (focusWindow *)pCaller; p->m_Imager->GetFrameSize(*x, *y); USHORT *pusData = new USHORT[(*x)*(*y)]; p->m_Imager->imagerStartCollect(); p->m_Imager->getFrame(pusData); p->m_Imager->imagerStopCollect(); BYTE *pbData = (BYTE*)pusData; *pvdata = pbData; return true; } void focusWindow::onConnectMotor() { if (ui.is_new_version_radioButton->isChecked()) { FileOperation* fileOperation = new FileOperation(); string directory = fileOperation->getDirectoryOfExe(); QString configFilePath = QString::fromStdString(directory) + "\\oneMotorConfigFile_focus.cfg"; m_multiAxisController = new IrisMultiMotorController(configFilePath); m_multiAxisController->moveToThread(&m_motorThread); connect(&m_motorThread, SIGNAL(finished()), m_multiAxisController, SLOT(deleteLater())); connect(this, SIGNAL(rmoveSignal(int, double, double, int)), m_multiAxisController, SLOT(rmove(int, double, double, int))); connect(this, SIGNAL(move2LocSignal(int, double, double, int)), m_multiAxisController, SLOT(moveTo(int, double, double, int))); connect(this, SIGNAL(rangeMeasurementSignal(int, double, int)), m_multiAxisController, SLOT(rangeMeasurement(int, double, int))); connect(this, SIGNAL(zeroStartSignal(int)), m_multiAxisController, SLOT(zeroStart(int))); connect(this, SIGNAL(move2MaxLocSignal(int, double, int)), m_multiAxisController, SLOT(moveToMax(int, double, int))); connect(m_multiAxisController, SIGNAL(broadcastLocationSignal(std::vector)), this, SLOT(display_x_loc(std::vector))); connect(m_multiAxisController, SIGNAL(motorStopSignal(int, double)), this, SLOT(moveAfterAutoFocus(int, double))); m_motorThread.start(); //归零 //emit zeroStartSignal(0); //自动调焦逻辑 m_coordinator = new MotionCaptureCoordinator(m_multiAxisController, m_Imager); m_coordinator->moveToThread(&m_MotionCaptureCoordinatorThread); connect(&m_MotionCaptureCoordinatorThread, SIGNAL(finished()), m_coordinator, SLOT(deleteLater())); connect(this, SIGNAL(startStepMotion(double, int, double, double)), m_coordinator, SLOT(startStepMotion(double, int, double, double))); connect(m_coordinator, SIGNAL(progressChanged(int)), this, SLOT(onAutoFocusProgress(int))); connect(m_coordinator, SIGNAL(sequenceComplete()), this, SLOT(onAutoFocusFinished())); m_MotionCaptureCoordinatorThread.start(); } else { if (m_ctrlFocusMotor != nullptr) { printf("不能重复连接-------------------------\n"); return; } bool isUltrasound = ui.ultrasound_radioButton->isChecked(); QString motorPortTmp = ui.motorPort_comboBox->currentText(); QString ultrasoundPortTmp = ui.ultrasoundPort_comboBox->currentText(); QRegExp rx("\\d+$"); rx.indexIn(motorPortTmp, 0); int motorPort = rx.cap(0).toInt(); rx.indexIn(ultrasoundPortTmp, 0); int ultrasoundPort = rx.cap(0).toInt(); if (isUltrasound) { PortInfo motor; motor.iPortType = 0; motor.indexParity = 0; motor.iPortNumber = motorPort; motor.indexBaudRate = 0x13; motor.indexBytesize = 3; motor.indexStopBits = 0; PortInfo ultrasound; ultrasound.iPortType = 0; ultrasound.indexParity = 0; ultrasound.iPortNumber = ultrasoundPort; ultrasound.indexBaudRate = 0x0C; ultrasound.indexBytesize = 3; ultrasound.indexStopBits = 0; m_ctrlFocusMotor = new CFocusMotorControl(); m_ctrlFocusMotor->SetLogicZero(m_iMinPos); m_ctrlFocusMotor->SetLimit(m_iMinPos, m_iMaxPos); m_ctrlFocusMotor->InitSystem(motor, ultrasound, test, this); //m_ctrlFocusMotor->MoveToLogicZero(); } else { PortInfo motor; motor.iPortType = 0; motor.indexParity = 0; motor.iPortNumber = motorPort; motor.indexBaudRate = 0x13; motor.indexBytesize = 3; motor.indexStopBits = 0; m_ctrlFocusMotor = new CFocusMotorControl(); m_ctrlFocusMotor->SetLogicZero(m_iMinPos); m_ctrlFocusMotor->SetLimit(m_iMinPos, m_iMaxPos); m_ctrlFocusMotor->InitSystem(motor, test, this); m_ctrlFocusMotor->MoveToLogicZero(); } } disableBeforeConnect(false); } void focusWindow::display_x_loc(std::vector loc) { double tmp = round(loc[0] * 100) / 100; this->ui.currentLocation_lineEdit->setText(QString::number(tmp)); } void focusWindow::onx_rangeMeasurement() { emit rangeMeasurementSignal(0, m_dSpeed, 1000); } void focusWindow::onMove2MotorLogicZero() { if (ui.is_new_version_radioButton->isChecked()) { emit zeroStartSignal(0); } else { m_ctrlFocusMotor->MoveToLogicZero(); } } void focusWindow::onMove2MotorMax() { if (ui.is_new_version_radioButton->isChecked()) { emit move2MaxLocSignal(0, m_dSpeed, 1000); } else { m_ctrlFocusMotor->MoveToPos(m_iMaxPos); } } void focusWindow::onAdd() { double rdistance = ui.addStepSize_lineEdit->text().toDouble(); if (ui.is_new_version_radioButton->isChecked()) { emit rmoveSignal(0, (double)abs(rdistance), m_dSpeed, 1000); } else { DriverInfo di; m_ctrlFocusMotor->GetDriverStatus(di); m_ctrlFocusMotor->MoveToPos(di.iAbsPosition + rdistance); } } void focusWindow::onSubtract() { double rdistance = ui.subtractStepSize_lineEdit->text().toDouble(); if (ui.is_new_version_radioButton->isChecked()) { emit rmoveSignal(0, (double)abs(rdistance) * -1, m_dSpeed, 1000); } else { DriverInfo di; m_ctrlFocusMotor->GetDriverStatus(di); m_ctrlFocusMotor->MoveToPos(di.iAbsPosition - rdistance); } } void focusWindow::onAutoFocus() { if (ui.is_new_version_radioButton->isChecked()) { //先按照一定间隔从负极限到正极限获取一系列（位置，调焦指数） m_iStepSize = ui.sample_ratio_lineEdit->text().toInt(); ui.autoFocusProgress_progressBar->setMinimum(0); ui.autoFocusProgress_progressBar->setMaximum(m_iStepSize); ui.autoFocusProgress_progressBar->reset(); //获取马达最大位置 std::vector maxRangeLocations = m_multiAxisController->getMaxPos(); double maxPos = maxRangeLocations[0]; emit startStepMotion(m_dSpeed, m_iStepSize, 0, maxPos); } else { bool isUltrasound = ui.ultrasound_radioButton->isChecked(); WorkerThread2* thread1 = new WorkerThread2(m_ctrlFocusMotor, isUltrasound); connect(thread1, SIGNAL(AutoFocusFinishedSignal()), this, SLOT(onAutoFocusFinished())); thread1->start(); WorkerThread4* progressThread = new WorkerThread4(m_ctrlFocusMotor); connect(progressThread, SIGNAL(AutoFocusProgressSignal(int)), this, SLOT(onAutoFocusProgress(int))); progressThread->start(); ui.autoFocusProgress_progressBar->reset(); } this->setDisabled(true); } void focusWindow::onManualFocus() { if (ui.is_new_version_radioButton->isChecked()) { } else { } m_FocusState += 1; std::cout << "点击调焦按钮！" << std::endl; if (m_FocusState % 2 == 1) { //开始调焦 emit StartManualFocusSignal(1); ui.manualFocus_btn->setText(QString::fromLocal8Bit("停止调焦")); ui.manualFocus_btn->setStyleSheet("QWidget{background-color:rgb(255,0,0);}"); ui.autoFocus_btn->setDisabled(true); //std::cout << "------------------------------------------：" << m_FocusState << std::endl; } else { emit StartManualFocusSignal(0); m_Imager->setFocusControlState(false); ui.manualFocus_btn->setText(QString::fromLocal8Bit("调焦")); ui.manualFocus_btn->setStyleSheet("QWidget{background-color:rgb(0,255,0);}"); ui.autoFocus_btn->setDisabled(false); } } void focusWindow::onUpdateCurrentLocation() { if (ui.is_new_version_radioButton->isChecked()) { //因为新版的马达控制是实时反馈位置信息，所以不需要做任何事 } else { DriverInfo di; m_ctrlFocusMotor->GetDriverStatus(di); ui.currentLocation_lineEdit->setText(QString::number(di.iAbsPosition)); } } void focusWindow::onMoveto() { double pos = ui.move2_lineEdit->text().toDouble(); if (ui.is_new_version_radioButton->isChecked()) { emit move2LocSignal(0, (double)pos, m_dSpeed, 1000); } else { //需要做参数有效性验证，查看是否在有效范围内 m_ctrlFocusMotor->MoveToPos(pos); } } void focusWindow::onAutoFocusFinished() { if (ui.is_new_version_radioButton->isChecked()) { //通过高斯拟合获取最佳位置 QVector positionData = m_coordinator->getAllPositionData(); std::vector actualPositions; std::vector cameraIndices; actualPositions.reserve(positionData.size()); cameraIndices.reserve(positionData.size()); int scaleFactor = 100; for (const auto& data : positionData) { actualPositions.push_back(data.actualPosition * scaleFactor); cameraIndices.push_back(data.cameraIndex); } double a_init, mu_init, sigma_init, c_init; getGaussianInitParam(actualPositions, cameraIndices, a_init, mu_init, sigma_init, c_init); double a = a_init, mu = mu_init, sigma = sigma_init, c = c_init; gaussian_fit(actualPositions, cameraIndices, a, mu, sigma, c); mu_init = mu_init / scaleFactor; mu = mu / scaleFactor; std::cout << "mu 初值：" << mu_init << std::endl << "mu 拟合值：" << mu << std::endl; //对拟合值进行判断，排除错误拟合情况（地物没有纹理） std::vector maxRangeLocations = m_multiAxisController->getMaxPos(); double maxPos = maxRangeLocations[0]; if (mu < 0 || mu > maxPos) { std::cout << "拟合失败！！！！！" << std::endl; m_goodPos = mu_init; m_isAutoFocusSuccess = false; } else { std::cout << "拟合成功！！！！！" << std::endl; m_goodPos = mu; m_isAutoFocusSuccess = true; } FileOperation* fileOperation = new FileOperation(); string directory = fileOperation->getDirectoryOfExe(); QDateTime now = QDateTime::currentDateTime(); QString format1 = "yyyyMMdd_HHmmss"; QString fileName = now.toString("yyyyMMdd_HHmmss"); QString fitDataFile = QDir::cleanPath(QString::fromStdString(directory) + QDir::separator() + fileName + "_" + QString::number(m_iStepSize) + "interval_" + QString::number(mu) + ".csv"); m_coordinator->saveToCsv(fitDataFile); //由于马达移动准确性较低，自动调焦完成后，直接移动到m_goodPos效果不好，所以先移动到tmpPos，然后移动到m_goodPos double tmpPos = m_goodPos - positionData[positionData.size() - 1].actualPosition / 10; m_isMoveAfterAutoFocus = true; emit move2LocSignal(0, (double)tmpPos, m_dSpeed, 1000); } else { onUpdateCurrentLocation(); } this->setDisabled(false); } void focusWindow::moveAfterAutoFocus(int motorID, double location) { if (!m_isMoveAfterAutoFocus) { return; } std::cout << "\n已经到达位置：" << location << std::endl; //移动马达到最佳位置 emit move2LocSignal(0, (double)m_goodPos, m_dSpeed, 1000); double tmp = abs(location - m_goodPos) / m_goodPos * 100; if (tmp<5) { if (!m_isAutoFocusSuccess) { QMessageBox msgBox; msgBox.setText(QString::fromLocal8Bit("纹理较弱，自动调焦效果不佳！请使用调焦纸进行自动调焦！")); msgBox.exec(); } else { QMessageBox msgBox; msgBox.setText(QString::fromLocal8Bit("自动调焦成功！")); msgBox.exec(); } m_isMoveAfterAutoFocus = false; } } void focusWindow::getGaussianInitParam(const std::vector& pos, const std::vector& index, double& a_init, double& mu_init, double& sigma_init, double& c_init) { auto minmax_element = std::minmax_element(index.begin(), index.end()); a_init = *minmax_element.second - *minmax_element.first; mu_init = pos[std::distance(index.begin(), minmax_element.second)]; c_init = *minmax_element.first; //sigma_init double half_max = (*minmax_element.second + *minmax_element.first) / 2.0; size_t peak_idx = std::distance(index.begin(), minmax_element.second); size_t left_idx = peak_idx;// 找左半高点 while (left_idx > 0 && index[left_idx] > half_max) left_idx--; size_t right_idx = peak_idx;// 找右半高点 while (right_idx < index.size() - 1 && index[right_idx] > half_max) right_idx++; double fwhm = pos[right_idx] - pos[left_idx]; sigma_init = fwhm / 2.3548; } // 使用 Gauss-Newton 进行高斯拟合 void focusWindow::gaussian_fit(const std::vector& x_data, const std::vector& y_data, double& a, double& mu, double& sigma, double& c) { const int max_iter = 100; for (int iter = 0; iter < max_iter; iter++) { Eigen::MatrixXd J(x_data.size(), 4); Eigen::VectorXd r(x_data.size()); for (size_t i = 0; i < x_data.size(); i++) { double xi = x_data[i]; double yi = y_data[i]; double exp_part = std::exp(-(xi - mu) * (xi - mu) / (2 * sigma * sigma)); double fi = a * exp_part + c; r(i) = yi - fi; // 雅可比 J(i, 0) = -exp_part; // ∂f/∂a J(i, 1) = -a * exp_part * ((xi - mu) / (sigma * sigma)); // ∂f/∂mu J(i, 2) = -a * exp_part * ((xi - mu) * (xi - mu) / (sigma * sigma * sigma)); // ∂f/∂sigma J(i, 3) = -1.0; // ∂f/∂c } Eigen::VectorXd delta = (J.transpose() * J).ldlt().solve(-J.transpose() * r); a += delta(0); mu += delta(1); sigma += delta(2); c += delta(3); if (delta.norm() < 1e-8) break; } } void focusWindow::onAutoFocusProgress(int progress) { //std::cout << "进度：" << progress << std::endl; ui.autoFocusProgress_progressBar->setValue(progress); } void focusWindow::onUltrasound_radioButton() { bool isUltrasound = ui.ultrasound_radioButton->isChecked(); if (isUltrasound) { ui.ultrasoundPort_comboBox->setEnabled(true); } else { ui.ultrasoundPort_comboBox->setEnabled(false); } } WorkerThread2::WorkerThread2(CFocusMotorControl * ctrlFocusMotor, bool isUltrasound) { m_ctrlFocusMotor = ctrlFocusMotor; m_bIsUltrasound = isUltrasound; } void WorkerThread2::run() { //配置文件 string HPPACfgFile = getPathofEXE() + "\\HPPA.cfg"; Configfile configfile; configfile.setConfigfilePath(HPPACfgFile); if (!configfile.isConfigfileExist()) configfile.createConfigFile(); configfile.parseConfigfile(); int coarse, fine; configfile.getTuningStepSize(coarse, fine); float fa, fb; configfile.getFitParams(fa, fb); int max_FocusRange, min_FocusRange; configfile.getAutoFocusRange(max_FocusRange, min_FocusRange); if (m_bIsUltrasound) { m_ctrlFocusMotor->SetFitParams(fa, fb); m_ctrlFocusMotor->StartAutoFocus(5, 2); } else { m_ctrlFocusMotor->StartAutoFocus(min_FocusRange, max_FocusRange, coarse, fine); } emit AutoFocusFinishedSignal(); } WorkerThread4::WorkerThread4(CFocusMotorControl * ctrlFocusMotor) { m_ctrlFocusMotor = ctrlFocusMotor; } void WorkerThread4::run() { while (true) { int progress = m_ctrlFocusMotor->GetProgressIndex(); //std::cout << "WorkerThread4::run----自动调焦进度：" << progress << std::endl; emit AutoFocusProgressSignal(progress); if (progress == 100) { //std::cout << "自动调焦完成！" << std::endl; break; } msleep(200); } } //------------------------------------------------------------------------------------------------------------------------------------------------------------- MotionCaptureCoordinator::MotionCaptureCoordinator( IrisMultiMotorController* motorCtrl, ImagerOperationBase* cameraCtrl, QObject* parent) : QObject(parent) , m_motorCtrl(motorCtrl) , m_cameraCtrl(cameraCtrl) , m_currentPos(0) , m_endPos(0) , m_isRunning(false) { //这些信号槽是按照逻辑顺序的 connect(this, SIGNAL(moveTo(int, double, double, int)), m_motorCtrl, SLOT(moveTo(int, double, double, int))); connect(m_motorCtrl, &IrisMultiMotorController::motorStopSignal, this, &MotionCaptureCoordinator::handlePositionReached); //connect(m_motorCtrl, &IrisMultiMotorController::moveFailed, // this, &MotionCaptureCoordinator::handleError); connect(this, SIGNAL(getFocusIndexSobel()), m_cameraCtrl, SLOT(getFocusIndexSobel())); connect(m_cameraCtrl, &ImagerOperationBase::FocusIndexSobelSignal, this, &MotionCaptureCoordinator::handleCaptureComplete); //connect(m_cameraCtrl, &ImagerOperationBase::captureFailed, // this, &MotionCaptureCoordinator::handleError); } MotionCaptureCoordinator::~MotionCaptureCoordinator() { } void MotionCaptureCoordinator::startStepMotion(double speed, int stepInterval, double startPos, double endPos) { QMutexLocker locker(&m_dataMutex); if (m_isRunning) { emit errorOccurred("Sequence already running"); return; } m_counter = 0; m_positionData.clear(); m_speed = speed; m_iStepInterval = stepInterval; m_iStepIntervalRealTime = 1; m_currentPos = startPos; m_endPos = endPos; m_posInternal = (endPos - startPos) / stepInterval; m_isRunning = true; processNextPosition(); } void MotionCaptureCoordinator::stopStepMotion() { QMutexLocker locker(&m_dataMutex); m_isRunning = false; emit sequenceStopped(); } QVector MotionCaptureCoordinator::getAllPositionData() const { //QMutexLocker locker(&m_dataMutex); return m_positionData; } bool MotionCaptureCoordinator::saveToCsv(const QString& filename) { //QMutexLocker locker(&m_dataMutex); QFile file(filename); if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) { return false; } QTextStream out(&file); out << "Timestamp,targetPosition,ActualPosition,FocusIndex\n"; for (const auto& data : m_positionData) { out << data.timestamp.toString("yyyy-MM-dd HH:mm:ss.zzz") << "," << QString::number(data.targetPosition, 'f', 4) << "," << QString::number(data.actualPosition, 'f', 4) << "," << QString::number(data.cameraIndex, 'f', 4) << "\n"; } file.close(); return true; } void MotionCaptureCoordinator::handlePositionReached(int motorID, double pos) { if (!m_isRunning) return; QMutexLocker locker(&m_dataMutex); //验证马达运动位置是否到达指定位置 //if (pos != m_currentPos) return; // 记录位置信息 PositionData data; data.targetPosition = m_currentPos; data.actualPosition = pos; data.timestamp = QDateTime::currentDateTime(); m_positionData.append(data); // 开始采集 emit getFocusIndexSobel(); } void MotionCaptureCoordinator::handleCaptureComplete(double index) { if (!m_isRunning) return; QMutexLocker locker(&m_dataMutex); // 更新最近一条记录的相机指数 //if (!m_positionData.isEmpty() && // m_positionData.last().targetPosition == m_positionData.last().actualPosition) //{ // m_positionData.last().cameraIndex = index; //} m_positionData.last().cameraIndex = index; m_currentPos += m_posInternal; m_iStepIntervalRealTime++; emit progressChanged(m_iStepIntervalRealTime); m_counter += 1; std::cout << "第" << m_counter << "次采集：" << std::endl; std::cout << "目标位置：" << m_positionData.last().targetPosition << std::endl; std::cout << "实际位置：" << m_positionData.last().actualPosition << std::endl; processNextPosition(); } void MotionCaptureCoordinator::handleError(const QString& error) { QMutexLocker locker(&m_dataMutex); m_isRunning = false; emit errorOccurred(error); } void MotionCaptureCoordinator::processNextPosition() { if (!m_isRunning) return; if (m_currentPos > m_endPos) { m_isRunning = false; emit sequenceComplete(); return; } emit moveTo(0, m_currentPos, m_speed, 1000); }