SE-2334 added support for RGBd (#8)

labforge · Jul 11, 2023 · cc46398 · cc46398
1 parent d3b7e3e
commit cc46398
Show file tree

Hide file tree

Showing 6 changed files with 78 additions and 36 deletions.
diff --git a/stereo_viewer/inc/gev/pipeline.hpp b/stereo_viewer/inc/gev/pipeline.hpp
@@ -45,7 +45,7 @@ namespace labforge::gev {
  void run() override;
 
  Q_SIGNALS:
- void pairReceived();
+ void pairReceived(bool is_disparity);
  void monoReceived(bool is_disparity);
  void terminated(bool fatal = false);
 

diff --git a/stereo_viewer/inc/ui/MainWindow.hpp b/stereo_viewer/inc/ui/MainWindow.hpp
@@ -48,7 +48,7 @@ public Q_SLOTS:
  void handleConnect();
  void handleDisconnect();
  void handleRecording();
- void handleData();
+ void handleStereoData(bool is_disparity);
  void handleMonoData(bool is_disparity);
  void newData(QImage &left, QImage &right, bool stereo=true, bool disparity=true);
  void onFolderSelect();

diff --git a/stereo_viewer/src/gev/pipeline.cc b/stereo_viewer/src/gev/pipeline.cc
@@ -174,22 +174,25 @@ void Pipeline::run() {
  img0 = lBuffer->GetMultiPartContainer()->GetPart(0)->GetImage();
  img1 = lBuffer->GetMultiPartContainer()->GetPart(1)->GetImage();
  m_pixformat->GetValue( pixformat );
- if(strcmp(pixformat.GetAscii(), "YUV422_8") != 0){
- break;
- }
+
  // Protected image creation
  {
+ int cv_pixfmt0 = (img0->GetPixelType() == PvPixelYUV422_8)? CV_8UC2: CV_16UC1;
+ int cv_pixfmt1 = (img1->GetPixelType() == PvPixelYUV422_8)? CV_8UC2: CV_16UC1;
+ is_disparity = ((cv_pixfmt0 == CV_16UC1) || (cv_pixfmt1 == CV_16UC1));
+
  QMutexLocker l(&m_image_lock);
  // See if there is chunk data attached
 
  m_images.push_back(
  make_tuple(
- new Mat(img0->GetHeight(), img0->GetWidth(), CV_8UC2, img0->GetDataPointer()),
- new Mat(img1->GetHeight(), img1->GetWidth(), CV_8UC2, img1->GetDataPointer())
+ new Mat(img0->GetHeight(), img0->GetWidth(), cv_pixfmt0, img0->GetDataPointer()),
+ new Mat(img1->GetHeight(), img1->GetWidth(), cv_pixfmt1, img1->GetDataPointer())
  )
  );
  }
- emit pairReceived();
+
+ emit pairReceived(is_disparity);
  break;
 
  case PvPayloadTypeImage: 

diff --git a/stereo_viewer/src/io/data_thread.cc b/stereo_viewer/src/io/data_thread.cc
@@ -51,13 +51,13 @@ void DataThread::process(const QImage &left_image, const QImage &right_image){
  QMutexLocker locker(&m_mutex);
  //-check queue size 
  if(m_queue.size() < MAX_QUEUE_SIZE){
-  m_queue.enqueue({left_image, right_image});
+ m_queue.enqueue({left_image, right_image});
  }
 
  if (!isRunning()) {
-  start(LowPriority);
+ start(LowPriority);
  } else { 
-  m_condition.wakeOne();
+ m_condition.wakeOne();
  }
 }
 
@@ -83,12 +83,19 @@ bool DataThread::setFolder(QString new_folder){
  } 
 
  if(m_stereo){
- status = getFilename(m_left_fname, m_folder, m_left_subfolder, "left_");
- status = status && getFilename(m_right_fname, m_folder, m_right_subfolder, "right_");
- } else if(m_disparity){
- status = getFilename(m_disparity_fname, m_folder, m_disparity_subfolder, "disparity_");
- } else {
- status = getFilename(m_left_fname, m_folder, m_left_subfolder, "mono_");
+ if(m_disparity){
+ status = getFilename(m_left_fname, m_folder, m_left_subfolder, "left_");
+ status = status && getFilename(m_disparity_fname, m_folder, m_disparity_subfolder, "disparity_");
+ }else{
+ status = getFilename(m_left_fname, m_folder, m_left_subfolder, "left_");
+ status = status && getFilename(m_right_fname, m_folder, m_right_subfolder, "right_");
+ }
+ }else{
+ if(m_disparity){
+ status = getFilename(m_disparity_fname, m_folder, m_disparity_subfolder, "disparity_");
+ } else {
+ status = getFilename(m_left_fname, m_folder, m_left_subfolder, "mono_");
+ }
  }
 
  return status;
@@ -111,14 +118,21 @@ void DataThread::run() {
  m_mutex.unlock();
 
  QString suffix = QString::number(m_frame_counter) + ".png"; 
- if(m_stereo){
+ if (m_stereo){
+ if(m_disparity){
  imdata.left.save(m_left_fname + suffix, "PNG");
- imdata.right.save(m_right_fname + suffix, "PNG"); 
- } else if(m_disparity){
- imdata.left.save(m_disparity_fname + suffix, "PNG");
+ imdata.right.save(m_disparity_fname + suffix, "PNG"); 
+ } else {
+ imdata.left.save(m_left_fname + suffix, "PNG");
+ imdata.right.save(m_right_fname + suffix, "PNG"); 
+ }
  } else {
- imdata.left.save(m_left_fname + suffix, "PNG");
- }
+ if(m_disparity){
+ imdata.left.save(m_disparity_fname + suffix, "PNG");
+ } else {
+ imdata.left.save(m_left_fname + suffix, "PNG");
+ }
+ } 
 
  m_frame_counter += 1; 
  }

diff --git a/stereo_viewer/src/viewer.cc b/stereo_viewer/src/viewer.cc
@@ -480,7 +480,7 @@ bool MainWindow::connectGEV(const PvDeviceInfo *info) {
  connect(m_pipeline.get(),
  &Pipeline::pairReceived,
  this,
- &MainWindow::handleData,
+ &MainWindow::handleStereoData,
  Qt::QueuedConnection);
  connect(m_pipeline.get(),
  &Pipeline::monoReceived,
@@ -509,12 +509,19 @@ void MainWindow::newData(QImage &left, QImage &right, bool stereo, bool disparit
  // Set the image
  cfg.widgetLeftSensor->setImage(left, false);
  cfg.widgetRightSensor->setVisible(stereo);
- cfg.chkColormap->setVisible((!stereo && disparity));
- cfg.labelColormap->setVisible((!stereo && disparity));
- cfg.cbxColormap->setVisible((!stereo && disparity));
+ cfg.lblDisplayRight->setVisible(stereo);
+
+ cfg.chkColormap->setVisible(disparity);
+ cfg.labelColormap->setVisible(disparity);
+ cfg.cbxColormap->setVisible(disparity);
 
  if(stereo){ 
  cfg.widgetRightSensor->setImage(right, false);
+ QString label = disparity?"Disparity":"Right";
+ cfg.lblDisplayRight->setText(label);
+ }else{
+ QString label = disparity?"Disparity":"Display";
+ cfg.lblDisplayLeft->setText(label);
  }
 
  // Do we have boundingboxes or feature points
@@ -542,17 +549,24 @@ void MainWindow::newData(QImage &left, QImage &right, bool stereo, bool disparit
 
 }
 
-void MainWindow::handleData() {
+void MainWindow::handleStereoData(bool is_disparity) {
  if(m_pipeline) {
  list<tuple<Mat*, Mat*>> images;
  m_pipeline->GetPairs(images);
- m_data_thread->setStereoDisparity(true,false);
+ m_data_thread->setStereoDisparity(true, is_disparity);
 
  // Convert and display
  for (auto it = images.begin(); it != images.end(); ++it) {
  QImage q1 = s_yuv2_to_qimage(get<0>(*it));
- QImage q2 = s_yuv2_to_qimage(get<1>(*it));
- newData(q1, q2, true, false);
+ QImage q2;
+
+ if(is_disparity){ 
+ q2 = s_mono_to_qimage(get<1>(*it), cfg.chkColormap->isChecked(), cfg.cbxColormap->currentIndex());
+ }else{
+ q2 = s_yuv2_to_qimage(get<1>(*it));
+ }
+
+ newData(q1, q2, true, is_disparity);
  delete get<0>(*it);
  delete get<1>(*it);
  }

diff --git a/stereo_viewer/stereo_viewer.ui b/stereo_viewer/stereo_viewer.ui
@@ -56,11 +56,22 @@
  <number>5</number>
  </property>
  <item>
- <widget class="QLabel" name="labelDisplay">
- <property name="text">
- <string>Display</string>
- </property>
- </widget>
+ <layout class="QHBoxLayout" name="horizontalLayout_2">
+ <item>
+ <widget class="QLabel" name="lblDisplayLeft">
+ <property name="text">
+ <string>Left</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="lblDisplayRight">
+ <property name="text">
+ <string>Right</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
  </item>
  <item>
  <widget class="QFrame" name="frameView">