diff --git a/HPPA/FocusDialog.ui b/HPPA/FocusDialog.ui index 6ce3145..c90bc8b 100644 --- a/HPPA/FocusDialog.ui +++ b/HPPA/FocusDialog.ui @@ -6,8 +6,8 @@ 0 0 - 600 - 332 + 632 + 444 @@ -41,13 +41,10 @@ - + 超声 - - Qt::AlignCenter - @@ -57,14 +54,14 @@ - - + + - 超声 + 新版 - + 连接线性平台 @@ -81,20 +78,49 @@ + + + true + + + 采样率 + + + Qt::AlignCenter + + + + + + + + 0 + 0 + + + + 20 + + + Qt::AlignCenter + + + + 24 - + 自动调焦 - + Qt::Horizontal @@ -107,7 +133,7 @@ - + 手动调焦 @@ -132,16 +158,16 @@ - 更新 + 更新实时位置 - + 0 - 46 + 0 @@ -150,75 +176,101 @@ Qt::AlignCenter + + true + - + 移动至 - + + + + + 0 + 0 + + + + 10 + + + Qt::AlignCenter + + + + + - + 0 - 46 + 0 - 50 + 10 Qt::AlignCenter - - - - LogicZero - - - - + - - + 0 - 46 + 0 - 50 + 10 Qt::AlignCenter - + + + + LogicZero + + + + max + + + + 量程测量 + + + diff --git a/HPPA/ImagerOperationBase.cpp b/HPPA/ImagerOperationBase.cpp index 2385968..ff9ba72 100644 --- a/HPPA/ImagerOperationBase.cpp +++ b/HPPA/ImagerOperationBase.cpp @@ -74,6 +74,8 @@ double ImagerOperationBase::auto_exposure() void ImagerOperationBase::focus() { + m_iFocusFramesNumber = 0; + m_iFocusFrameCounter = 1; //std::cout << "-----------" << std::endl; @@ -85,16 +87,42 @@ void ImagerOperationBase::focus() auto_exposure(); std::cout << "õعʱΪ" << getIntegrationTime() << std::endl; + int iWidth, iHeight; + GetFrameSize(iWidth, iHeight); + unsigned short* tmp = new unsigned short[m_FrameSize]; + imagerStartCollect(); //emit SpectralSignal(1); m_bFocusControlState = true; while (m_bFocusControlState) { + ////֡ƽ֡ + //memset((void*)buffer, 0, m_FrameSize * sizeof(unsigned short)); + //int fn = 5; + //for (int i = 0; i < fn; i++) + //{ + // getFrame(tmp); + + // for (int j = 0; j < m_FrameSize; j++) + // { + // buffer[j] += tmp[j]; + // } + //} + //for (int j = 0; j < m_FrameSize; j++) + //{ + // buffer[j] += buffer[j] / fn; + //} + getFrame(buffer); + //m_RgbImage->FillFocusGrayImage(buffer); m_RgbImage->FillFocusGrayQImage(buffer); + double focusIndex = calcFocusIndexSobelPrivate(buffer); + emit FocusIndexSobelSignal(focusIndex); + std::cout << "focusIndex" << focusIndex << std::endl; + emit SpectralSignal(1); ++m_iFocusFrameCounter; @@ -102,6 +130,7 @@ void ImagerOperationBase::focus() emit SpectralSignal(0); imagerStopCollect(); + delete[] tmp; setFramerate(tmpFrmerate); setIntegrationTime(tmpIntegrationTime); @@ -335,6 +364,61 @@ int ImagerOperationBase::GetFrameSize(int& iWidth, int& iHeight) return 0; } +void ImagerOperationBase::getFocusIndexSobel() +{ + imagerStartCollect(); + getFrame(buffer); + imagerStopCollect(); + + double focusIndex = calcFocusIndexSobelPrivate(buffer); + + emit FocusIndexSobelSignal(focusIndex); +} + +double ImagerOperationBase::calcFocusIndexSobelPrivate(void* pvData) +{ + int iSelection = 0; + int iWidth, iHeight; + GetFrameSize(iWidth, iHeight); + + unsigned short* psData; + psData = (unsigned short*)pvData; + + cv::Mat gray(iHeight, iWidth, CV_16UC1, psData);//֤gray.dataݺpsDataһ + /*string rgbFilePathNoStrech = "E:\\hppa\\delete\\focusImg_"; + string tmp1 = std::to_string(m_iFocusFramesNumber); + string tmp2 = ".png";*/ + + string rgbFilePathNoStrech = "E:\\hppa\\delete\\focusImg_" + std::to_string(m_iFocusFramesNumber) + ".png"; + + //cv::imwrite(rgbFilePathNoStrech, gray); + m_iFocusFramesNumber++; + + //˲ + //cv::Mat outputImage; + //cv::Size kernelSize(5, 5); + //double sigmaX = 1.5; + //cv::GaussianBlur(gray, outputImage, kernelSize, sigmaX); + + cv::Mat outputImage = gray; + + cv::Mat gradX, gradY, absGradX, absGradY; + + cv::Sobel(outputImage, gradX, CV_32F, 1, 0);//ΪCV_16Scv::magnitude + cv::Sobel(outputImage, gradY, CV_32F, 0, 1); + cv::convertScaleAbs(gradX, absGradX); + cv::convertScaleAbs(gradY, absGradY); + cv::Mat grad; + cv::addWeighted(absGradX, 0.5, absGradY, 0.5, 0, grad); + + cv::Mat magnitude, direction; + cv::magnitude(gradX, gradY, magnitude);// + cv::phase(gradX, gradY, direction, true); // trueʾؽǶȶǻ + + + return cv::mean(magnitude)[0]; +} + CImage* ImagerOperationBase::getRgbImage() const { return m_RgbImage; diff --git a/HPPA/ImagerOperationBase.h b/HPPA/ImagerOperationBase.h index 30d57fb..2e017d0 100644 --- a/HPPA/ImagerOperationBase.h +++ b/HPPA/ImagerOperationBase.h @@ -55,6 +55,7 @@ public: void setFocusControlState(bool FocusControlState); int GetFrameSize(int& iWidth, int& iHeight); + protected: CImage* m_RgbImage;//ʾrgbͼ @@ -85,6 +86,8 @@ protected: private: + int m_iFocusFramesNumber; + double calcFocusIndexSobelPrivate(void* pvData); public slots: virtual void connect_imager(int frameNumber);//ٻ @@ -93,6 +96,8 @@ public slots: virtual void start_record(); virtual void record_dark(); virtual void record_white(); + + void getFocusIndexSobel(); signals: void PlotSignal(int);//Ӱźţ-1˴βɼһλ void RecordFinishedSignal_WhenFrameNumberMeet();//ɼźţҪɼ֡m_iFrameNumberɼ @@ -102,6 +107,8 @@ signals: void RecordWhiteFinishSignal(); void RecordDarlFinishSignal(); + void FocusIndexSobelSignal(double); + void testImagerStatus();//ʾԲ״̬Ƿӣӳ }; diff --git a/HPPA/focusWindow.cpp b/HPPA/focusWindow.cpp index ba26738..8afbfda 100644 --- a/HPPA/focusWindow.cpp +++ b/HPPA/focusWindow.cpp @@ -1,11 +1,11 @@ -#include "stdafx.h" +#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); @@ -18,7 +18,7 @@ focusWindow::focusWindow(QWidget *parent, ImagerOperationBase* imager) disableBeforeConnect(true); - setAttribute(Qt::WA_DeleteOnClose);//ùرմ͵ô + setAttribute(Qt::WA_DeleteOnClose);//设置关闭窗体就调用窗体的析构函数 m_Imager = imager; m_FocusState = 0; @@ -38,7 +38,9 @@ focusWindow::focusWindow(QWidget *parent, ImagerOperationBase* imager) 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; @@ -51,20 +53,29 @@ focusWindow::focusWindow(QWidget *parent, ImagerOperationBase* imager) } } - //Զ + //设置自动调焦进度条 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);//ûûеֹͣ͹رմ + 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) @@ -92,142 +103,238 @@ bool test(void *pCaller, int *x, int *y, void **pvdata) void focusWindow::onConnectMotor() { - if (m_ctrlFocusMotor != nullptr) + if (ui.is_new_version_radioButton->isChecked()) { - printf("ظ-------------------------\n"); - return; - } + FileOperation* fileOperation = new FileOperation(); + string directory = fileOperation->getDirectoryOfExe(); + QString configFilePath = QString::fromStdString(directory) + "\\oneMotorConfigFile_focus.cfg"; - 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; + 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))); + m_motorThread.start(); - motor.iPortNumber = motorPort; - motor.indexBaudRate = 0x13; - motor.indexBytesize = 3; - motor.indexStopBits = 0; + //归零 + //emit zeroStartSignal(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(); + //自动调焦逻辑 + 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 { - PortInfo motor; - motor.iPortType = 0; - motor.indexParity = 0; - motor.iPortNumber = motorPort; - motor.indexBaudRate = 0x13; - motor.indexBytesize = 3; - motor.indexStopBits = 0; + if (m_ctrlFocusMotor != nullptr) + { + printf("不能重复连接-------------------------\n"); + return; + } - m_ctrlFocusMotor = new CFocusMotorControl(); + bool isUltrasound = ui.ultrasound_radioButton->isChecked(); - m_ctrlFocusMotor->SetLogicZero(m_iMinPos); - m_ctrlFocusMotor->SetLimit(m_iMinPos, m_iMaxPos); - m_ctrlFocusMotor->InitSystem(motor, test, this); - m_ctrlFocusMotor->MoveToLogicZero(); + 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() { - m_ctrlFocusMotor->MoveToLogicZero(); + if (ui.is_new_version_radioButton->isChecked()) + { + emit zeroStartSignal(0); + } + else + { + m_ctrlFocusMotor->MoveToLogicZero(); + } } void focusWindow::onMove2MotorMax() { - m_ctrlFocusMotor->MoveToPos(m_iMaxPos); + if (ui.is_new_version_radioButton->isChecked()) + { + emit move2MaxLocSignal(0, m_dSpeed, 1000); + } + else + { + m_ctrlFocusMotor->MoveToPos(m_iMaxPos); + } } void focusWindow::onAdd() { - DriverInfo di; - m_ctrlFocusMotor->GetDriverStatus(di); + double rdistance = ui.addStepSize_lineEdit->text().toDouble(); - int stepSize = ui.addStepSize_lineEdit->text().toInt(); + 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 + stepSize); + m_ctrlFocusMotor->MoveToPos(di.iAbsPosition + rdistance); + } } void focusWindow::onSubtract() { - DriverInfo di; - m_ctrlFocusMotor->GetDriverStatus(di); + double rdistance = ui.subtractStepSize_lineEdit->text().toDouble(); - int stepSize = ui.subtractStepSize_lineEdit->text().toInt(); + 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 - stepSize); + m_ctrlFocusMotor->MoveToPos(di.iAbsPosition - rdistance); + } } void focusWindow::onAutoFocus() { - bool isUltrasound = ui.ultrasound_radioButton->isChecked(); - WorkerThread2 *thread1 = new WorkerThread2(m_ctrlFocusMotor, isUltrasound); + 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(); - connect(thread1, SIGNAL(AutoFocusFinishedSignal()), this, SLOT(onAutoFocusFinished())); + //获取马达最大位置 + 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); - thread1->start(); + 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); - - WorkerThread4 *progressThread = new WorkerThread4(m_ctrlFocusMotor); - connect(progressThread, SIGNAL(AutoFocusProgressSignal(int)), this, SLOT(onAutoFocusProgress(int))); - progressThread->start(); - ui.autoFocusProgress_progressBar->reset(); } void focusWindow::onManualFocus() { + if (ui.is_new_version_radioButton->isChecked()) + { + + } + else + { + + } + m_FocusState += 1; - std::cout << "ť" << std::endl; + std::cout << "点击调焦按钮！" << std::endl; if (m_FocusState % 2 == 1) { - //ʼ + //开始调焦 emit StartManualFocusSignal(1); - ui.manualFocus_btn->setText(QString::fromLocal8Bit("ֹͣ")); + 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; + //std::cout << "------------------------------------------：" << m_FocusState << std::endl; } else { emit StartManualFocusSignal(0); m_Imager->setFocusControlState(false); - ui.manualFocus_btn->setText(QString::fromLocal8Bit("")); + ui.manualFocus_btn->setText(QString::fromLocal8Bit("调焦")); ui.manualFocus_btn->setStyleSheet("QWidget{background-color:rgb(0,255,0);}"); @@ -237,30 +344,157 @@ void focusWindow::onManualFocus() void focusWindow::onUpdateCurrentLocation() { - DriverInfo di; - m_ctrlFocusMotor->GetDriverStatus(di); - ui.currentLocation_lineEdit->setText(QString::number(di.iAbsPosition)); + 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() { - int pos = ui.currentLocation_lineEdit->text().toInt(); - //ҪЧ֤鿴ǷЧΧ + 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); + m_ctrlFocusMotor->MoveToPos(pos); + } } void focusWindow::onAutoFocusFinished() { - this->setDisabled(false); + if (ui.is_new_version_radioButton->isChecked()) + { + //通过高斯拟合获取最佳位置 + QVector positionData = m_coordinator->getAllPositionData(); - onUpdateCurrentLocation(); + 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; + QMessageBox msgBox; + msgBox.setText(QString::fromLocal8Bit("纹理较弱，自动调焦效果不佳！请使用调焦纸进行自动调焦！")); + msgBox.exec(); + mu = mu_init; + } + + FileOperation* fileOperation = new FileOperation(); + string directory = fileOperation->getDirectoryOfExe(); + QDateTime now = QDateTime::currentDateTime(); + QString format1 = "yyyyMMdd_HHmmss"; + QString fileName = now.toString("yyyyMMdd_HHmmss"); + + QString tmp = QDir::cleanPath(QString::fromStdString(directory) + QDir::separator() + fileName + "_" + QString::number(m_iStepSize) + "interval_" + QString::number(mu) + ".csv"); + + QString focusFilePath = QString::fromStdString(directory) + "\\focus.csv"; + m_coordinator->saveToCsv(tmp); + + //移动马达到最佳位置 + emit move2LocSignal(0, (double)mu, m_dSpeed, 1000); + } + else + { + onUpdateCurrentLocation(); + } + + this->setDisabled(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; + //std::cout << "进度：" << progress << std::endl; ui.autoFocusProgress_progressBar->setValue(progress); } @@ -287,7 +521,7 @@ WorkerThread2::WorkerThread2(CFocusMotorControl * ctrlFocusMotor, bool isUltraso void WorkerThread2::run() { - //ļ + //配置文件 string HPPACfgFile = getPathofEXE() + "\\HPPA.cfg"; Configfile configfile; configfile.setConfigfilePath(HPPACfgFile); @@ -326,15 +560,185 @@ void WorkerThread4::run() { int progress = m_ctrlFocusMotor->GetProgressIndex(); - //std::cout << "WorkerThread4::run----Զȣ" << progress << std::endl; + //std::cout << "WorkerThread4::run----自动调焦进度：" << progress << std::endl; emit AutoFocusProgressSignal(progress); if (progress == 100) { - //std::cout << "Զɣ" << std::endl; + //std::cout << "自动调焦完成！" << std::endl; break; } msleep(200); } -} \ No newline at end of file +} + + +//------------------------------------------------------------------------------------------------------------------------------------------------------------- + +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); +} diff --git a/HPPA/focusWindow.h b/HPPA/focusWindow.h index 2466a71..fc98a01 100644 --- a/HPPA/focusWindow.h +++ b/HPPA/focusWindow.h @@ -16,6 +16,8 @@ #include #include #include +#include +#include #include "ui_FocusDialog.h" #include "AbstractPortMiscDefines.h" @@ -26,6 +28,71 @@ #include "hppaConfigFile.h" #include "path_tc.h" #include "ImagerOperationBase.h" +#include "IrisMultiMotorController.h" + +#include +#include + +// ݼ¼ṹ +struct PositionData { + double targetPosition; // Ŀλ + double actualPosition; // ʵλ + double cameraIndex; // ɼָ + QDateTime timestamp; // ʱ + + PositionData(double target = 0, double actual = 0.0, double index = 0.0) + : targetPosition(target), actualPosition(actual), + cameraIndex(index), timestamp(QDateTime::currentDateTime()) {} +}; + +// Э +class MotionCaptureCoordinator : public QObject +{ + Q_OBJECT +public: + MotionCaptureCoordinator(IrisMultiMotorController* motorCtrl, + ImagerOperationBase* cameraCtrl, + QObject* parent = nullptr); + ~MotionCaptureCoordinator(); + + QVector getAllPositionData() const; + bool saveToCsv(const QString& filename); + +public slots: + void startStepMotion(double speed, int stepInterval = 100, double startPos = 0, double endPos = -1);//-1ܵԶλ + void stopStepMotion(); + +signals: + void progressChanged(int progress); + void sequenceComplete(); + void sequenceStopped(); + void errorOccurred(const QString& error); + void moveTo(int, double, double, int); + void getFocusIndexSobel(); + +private slots: + void handlePositionReached(int motorID, double pos); + void handleCaptureComplete(double index); + void handleError(const QString& error); + +private: + void processNextPosition(); + + IrisMultiMotorController* m_motorCtrl; + ImagerOperationBase* m_cameraCtrl; + QVector m_positionData; + mutable QMutex m_dataMutex; + + double m_posInternal; + double m_currentPos; + double m_endPos; + bool m_isRunning; + double m_speed; + + int m_iStepInterval; + int m_iStepIntervalRealTime; + int m_counter; +}; class focusWindow:public QDialog { @@ -50,6 +117,18 @@ private: void disableBeforeConnect(bool disable); + QThread m_motorThread; + IrisMultiMotorController* m_multiAxisController; + double m_dSpeed; + + QThread m_MotionCaptureCoordinatorThread; + MotionCaptureCoordinator* m_coordinator; + + int m_iStepSize; + void getGaussianInitParam(const std::vector& pos, const std::vector& index, double& a_init, double& mu_init, double& sigma_init, double& c_init); + void gaussian_fit(const std::vector& x_data, const std::vector& y_data, double& a, double& mu, double& sigma, double& c); + + public Q_SLOTS: void onConnectMotor(); void onMove2MotorLogicZero(); @@ -64,8 +143,19 @@ public Q_SLOTS: void onAutoFocusProgress(int progress); void onUltrasound_radioButton(); + void display_x_loc(std::vector loc); + void onx_rangeMeasurement(); + signals: void StartManualFocusSignal(int);//1ʼ0ֹͣ + + void move2LocSignal(int, double, double, int); + void move2MaxLocSignal(int, double, int); + void rmoveSignal(int, double, double, int); + void rangeMeasurementSignal(int, double, int); + void zeroStartSignal(int); + + void startStepMotion(double speed, int stepInterval = 100, double startPos = 0, double endPos = -1); }; class WorkerThread2 : public QThread diff --git a/cfg_file_backup/HPPA.cfg b/cfg_file_backup/HPPA.cfg new file mode 100644 index 0000000..e2055e8 --- /dev/null +++ b/cfg_file_backup/HPPA.cfg @@ -0,0 +1,43 @@ +SN = "2004"; +autoFocus : +{ + PositionRestriction : + { + max = 1000; + min = 120; + }; + TuningStepSize : + { + coarse = 10; + fine = 2; + }; + FitParams : + { + fa = 0.0017; + fb = 0.3277; + }; + AutoFocusRange : + { + max = 688; + min = 144; + }; +}; +motionPlatform : +{ + x : + { + StepAnglemar = 1.8; + Lead = 1.0; + SubdivisionMultiples = 8; + ScaleFactor = 1.0; + MaxRange = 30.742266; + }; + y : + { + StepAnglemar = 1.8; + Lead = 1.0; + SubdivisionMultiples = 8; + ScaleFactor = 1.0; + MaxRange = 31.283163; + }; +}; diff --git a/cfg_file_backup/oneMotorConfigFile.cfg b/cfg_file_backup/oneMotorConfigFile.cfg new file mode 100644 index 0000000..4b3f331 --- /dev/null +++ b/cfg_file_backup/oneMotorConfigFile.cfg @@ -0,0 +1,60 @@ +SN = "0"; +motors : +{ + motor1 : + { + platformParams : + { + hardwareParams : + { + StepAngle = 1.8; + Lead = 4.0; + ScaleFactor = 1.0; + }; + runParams : + { + RecordSpeed = 1.8; + MoveSpeed = 1.8; + ReturnSpeed = 1.8; + MaxRange = 120.0; + }; + }; + motorParams : + { + Manufacturer = 0; + CommunicationProtocol = 0; + connectionParams : + { + SerialPortNumber = "COM10"; + BaudRate = 9600; + }; + initParams : + { + limit : + { + msr = 1; + msv = 0; + psr = 2; + psv = 0; + }; + other : + { + acc = 20000.0; + cra = 4.0; + crh = 1.0; + crn = 4.0; + dec = 20000.0; + mcs = 7; + }; + zeroStart : + { + osv = 0; + snr = 0; + zmd = 2; + zsd = 3000; + zsp = 2400; + }; + }; + }; + }; +}; diff --git a/cfg_file_backup/oneMotorConfigFile_focus.cfg b/cfg_file_backup/oneMotorConfigFile_focus.cfg new file mode 100644 index 0000000..9c3b724 --- /dev/null +++ b/cfg_file_backup/oneMotorConfigFile_focus.cfg @@ -0,0 +1,60 @@ +SN = "0"; +motors : +{ + motor1 : + { + platformParams : + { + hardwareParams : + { + StepAngle = 1.8; + Lead = 1.37; + ScaleFactor = 1.0; + }; + runParams : + { + RecordSpeed = 1.0; + MoveSpeed = 1.0; + ReturnSpeed = 1.0; + MaxRange = 11.559696; + }; + }; + motorParams : + { + Manufacturer = 0; + CommunicationProtocol = 1; + connectionParams : + { + SerialPortNumber = "COM10"; + BaudRate = 9600; + }; + initParams : + { + limit : + { + msr = 1; + msv = 1; + psr = 2; + psv = 1; + }; + other : + { + acc = 19200.0; + cra = 0.2; + crh = 0.0; + crn = 0.2; + dec = 19200.0; + mcs = 6; + }; + zeroStart : + { + osv = 1; + snr = 0; + zmd = 1; + zsd = 4000; + zsp = 2400; + }; + }; + }; + }; +};