add stereo image pair loading & rgbd piping with embedded hfov in depth

src/aboutdialog.ui

@@ -6,8 +6,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>168</width>
-    <height>50</height>
+    <width>200</width>
+    <height>61</height>
    </rect>
   </property>
   <property name="sizePolicy">
@@ -39,7 +39,7 @@
    <item row="3" column="0">
     <widget class="QLabel" name="lblCompany">
      <property name="text">
-      <string>(C) Industrial 3D Robotics 2020</string>
+      <string>(C) Industrial 3D Robotics 2021</string>
      </property>
     </widget>
    </item>

src/camera/abstractstereocamera.cpp

@@ -453,6 +453,18 @@ cv::Mat AbstractStereoCamera::getDisparityFiltered(){
     disparity_mutex.unlock();
 }
 
+void AbstractStereoCamera::getDepth(cv::Mat &dst) {
+    disparity_mutex.lock();
+    depth.copyTo(dst);
+    disparity_mutex.unlock();
+}
+
+cv::Mat AbstractStereoCamera::getDepth(){
+    disparity_mutex.lock();
+    return depth.clone();
+    disparity_mutex.unlock();
+}
+
 void AbstractStereoCamera::generateRectificationMaps(cv::Size image_size){
     /*
     cv::Mat dl_camera_matrix = l_camera_matrix.clone();
@@ -879,7 +891,7 @@ void AbstractStereoCamera::processMatch(){
     if (left_output.empty() || right_output.empty()){
         return;
     }
-    cv::Mat left_img, right_img, disp, left_bgr, disp_filtered;
+    cv::Mat left_img, right_img, disp, left_bgr, disp_filtered, depth_tmp;
     left_img = left_output.clone();
     right_img = right_output.clone();
     lr_image_mutex.unlock();
@@ -899,15 +911,13 @@ void AbstractStereoCamera::processMatch(){
     disparity = disp.clone();
     CVSupport::removeInvalidDisparity(disparity/16,Q,disp_filtered);
     disparity_filtered = disp_filtered.clone();
+    CVSupport::disparity2Depth(disp_filtered, Q, depth_tmp, 10000, downsample_factor);
+    depth = depth_tmp.clone();
     disparity_mutex.unlock();
     //qDebug() << "Getting left image from stereo match...";
     left_bgr = matcher->getLeftBGRImage();
     emit matched();
 
-    //double min_disp, max_disp;
-    //CVSupport::getMinMaxDisparity(disp,min_disp,max_disp);
-    //qDebug() << min_disp << "," << max_disp;
-
     cv::Mat disp_colormap;
     if (video_src == VIDEO_SRC_DISPARITY || (reprojecting && getPointCloudTexture() == POINT_CLOUD_TEXTURE_DEPTH)){
         CVSupport::disparity2colormap(disp,Q,disp_colormap);
@@ -916,6 +926,7 @@ void AbstractStereoCamera::processMatch(){
             if (capturing_video)
                 addVideoStreamFrame(disp_colormap);
         }
+        //TODO add RGBD support for with VIDEO_SRC_RGBD
     }
 
     if (reprojecting){
@@ -927,11 +938,12 @@ void AbstractStereoCamera::processMatch(){
             disp_colormap.copyTo(texture_image);
         }
 
-        if (disp.empty() || texture_image.empty()){
+        if (depth_tmp.empty() || texture_image.empty()){
             return;
         }
 
-        pcl::PointCloud<pcl::PointXYZRGB>::Ptr ptCloudTemp = PCLSupport::disparity2PointCloud(disp,texture_image,Q, downsample_factor);
+        //pcl::PointCloud<pcl::PointXYZRGB>::Ptr ptCloudTemp = PCLSupport::disparity2PointCloud(disp,texture_image,Q, downsample_factor);
+        pcl::PointCloud<pcl::PointXYZRGB>::Ptr ptCloudTemp = PCLSupport::depth2PointCloud(depth_tmp,texture_image);
 
         pcl::PassThrough<pcl::PointXYZRGB> pass;
         pass.setInputCloud (ptCloudTemp);
@@ -1011,6 +1023,9 @@ bool AbstractStereoCamera::setVideoStreamParams(QString filename, int fps, int c
         case VIDEO_SRC_DISPARITY:
             file_prefix = "disparity_video_";
             break;
+        case VIDEO_SRC_RGBD:
+            file_prefix = "rgbd_video_";
+            break;
         }
         video_filename = QString("%1/%2%3.avi").arg(save_directory, file_prefix, date_string).toStdString();
     }

src/camera/abstractstereocamera.h

@@ -90,7 +90,7 @@ class AbstractStereoCamera : public QObject {
 
     enum PointCloudTexture { POINT_CLOUD_TEXTURE_IMAGE, POINT_CLOUD_TEXTURE_DEPTH };
 
-    enum VideoSource { VIDEO_SRC_STEREO, VIDEO_SRC_LEFT, VIDEO_SRC_RIGHT, VIDEO_SRC_DISPARITY };
+    enum VideoSource { VIDEO_SRC_STEREO, VIDEO_SRC_LEFT, VIDEO_SRC_RIGHT, VIDEO_SRC_DISPARITY, VIDEO_SRC_RGBD };
 
     //! Structure to hold camera settings
     struct StereoCameraSettings {
@@ -113,7 +113,6 @@ class AbstractStereoCamera : public QObject {
         std::string right_camera_serial;
         StereoCameraType camera_type; // type of camera
         std::string i3dr_serial; // defined i3dr serial for camera pair
-        std::string filename; // filename for video [only used for stereoCameraFromVideo]
     };
 
     explicit AbstractStereoCamera(StereoCameraSerialInfo serial_info,
@@ -266,6 +265,18 @@ class AbstractStereoCamera : public QObject {
   */
     void getDisparityFiltered(cv::Mat &dst);
 
+    //! Get the depth image
+    /*!
+  * @return OpenCV matrix containing depth image
+  */
+    cv::Mat getDepth();
+
+    //! Get the depth image
+    /*!
+  * @param[out] dst OpenCV matrix to store image into
+  */
+    void getDepth(cv::Mat &dst);
+
 
     //! Get a pointer to the current point cloud
     /*!
@@ -746,6 +757,7 @@ private slots:
 
     cv::Mat disparity;
     cv::Mat disparity_filtered;
+    cv::Mat depth;
 
     double visualisation_min_z = 0.2;
     double visualisation_max_z = 5;

src/camera/stereocamerafromimage.cpp

@@ -0,0 +1,93 @@
+/*
+* Copyright I3D Robotics Ltd, 2020
+* Author: Josh Veitch-Michaelis, Ben Knight ([email protected])
+*/
+
+#include "stereocamerafromimage.h"
+
+bool StereoCameraFromImage::openCamera(){
+    if (isConnected()){
+        closeCamera();
+    }
+    int fps = stereoCameraSettings_.fps;
+    std::string fname_l = stereoCameraSerialInfo_.left_camera_serial;
+    std::string fname_r = stereoCameraSerialInfo_.right_camera_serial;
+
+    left_raw = cv::imread(fname_l,cv::IMREAD_UNCHANGED);
+    right_raw = cv::imread(fname_r,cv::IMREAD_UNCHANGED);
+
+    //TODO check image files exist
+
+    image_height = left_raw.size().height;
+    image_width = left_raw.size().width;
+    image_bitdepth = 1; //TODO get bit depth
+    emit update_size(image_width, image_height, image_bitdepth);
+
+    setFPS(fps);
+
+    connected = true;
+    return connected;
+}
+
+bool StereoCameraFromImage::closeCamera(){
+    connected = false;
+    emit disconnected();
+    return true;
+}
+
+bool StereoCameraFromImage::captureSingle(){
+
+    // Simulate frame rate
+    double delay_needed = (1000.0/(video_fps+1)) - frame_timer.elapsed();
+    if(delay_needed > 0){
+        QThread::msleep(delay_needed);
+    }
+
+    frame_timer.restart();
+
+    //send_error(CAPTURE_ERROR);
+    //emit captured_fail();
+    emit captured_success();
+    emit captured();
+    return true;
+}
+
+void StereoCameraFromImage::captureThreaded(){
+    future = QtConcurrent::run(this, &StereoCameraFromImage::captureSingle);
+}
+
+bool StereoCameraFromImage::enableCapture(bool enable){
+    if (enable){
+        //Start capture thread
+        connect(this, SIGNAL(captured()), this, SLOT(captureThreaded()));
+        capturing = true;
+        captureThreaded();
+    } else {
+        //Stop capture thread
+        disconnect(this, SIGNAL(captured()), this, SLOT(captureThreaded()));
+        capturing = false;
+    }
+    return true;
+}
+
+std::vector<AbstractStereoCamera::StereoCameraSerialInfo> StereoCameraFromImage::listSystems(){
+    std::vector<AbstractStereoCamera::StereoCameraSerialInfo> connected_serial_infos;
+    return connected_serial_infos;
+}
+
+bool StereoCameraFromImage::setFPS(int fps){
+    if (!isCapturing()){
+        frame_rate = fps;
+        video_fps = fps;
+        return true;
+    } else {
+        qDebug() << "Cannot set FPS while capturing. Stop capturing and try again.";
+        return false;
+    }
+}
+
+StereoCameraFromImage::~StereoCameraFromImage(void) {
+    if (connected){
+        closeCamera();
+    }
+}

src/camera/stereocamerafromimage.h

@@ -0,0 +1,60 @@
+/*
+* Copyright I3D Robotics Ltd, 2020
+* Author: Josh Veitch-Michaelis, Ben Knight ([email protected])
+*/
+
+#ifndef STEREOCAMERAFROMIMAGE_H
+#define STEREOCAMERAFROMIMAGE_H
+
+#include <abstractstereocamera.h>
+#include <QThread>
+
+//!  Stereo image feed
+/*!
+  Vitual stereo camera from image feed
+*/
+
+class StereoCameraFromImage : public AbstractStereoCamera
+{
+Q_OBJECT
+
+public:
+
+    explicit StereoCameraFromImage(AbstractStereoCamera::StereoCameraSerialInfo serial_info,
+                                AbstractStereoCamera::StereoCameraSettings camera_settings,
+                                QObject *parent = 0) :
+                AbstractStereoCamera(serial_info, camera_settings, parent){
+    }
+
+    static std::vector<AbstractStereoCamera::StereoCameraSerialInfo> listSystems();
+
+    ~StereoCameraFromImage(void);
+
+public slots:
+    // Implimentations of virtual functions from parent class
+    bool openCamera();
+    bool closeCamera();
+    bool captureSingle();
+    bool enableCapture(bool enable);
+    bool setFPS(int fps);
+    bool setExposure(double){return false;} //NA
+    bool enableAutoExposure(bool){return false;} //NA
+    bool setPacketSize(int){return false;} //NA
+    bool setPacketDelay(int){return false;} //NA
+    bool enableTrigger(bool){return false;} //NA
+    bool enableAutoGain(bool){return false;} //NA
+    bool setGain(int){return false;} //NA
+    bool setBinning(int){return false;} //NA
+
+    void captureThreaded();
+
+signals:
+    void videoPosition(int);
+
+private:
+    QFuture<void> future;
+    QElapsedTimer frame_timer;
+    double video_fps;
+};
+
+#endif // STEREOCAMERAFROMIMAGE_H

src/camera/stereocamerafromvideo.cpp

@@ -10,7 +10,7 @@ bool StereoCameraFromVideo::openCamera(){
         closeCamera();
     }
     int fps = stereoCameraSettings_.fps;
-    std::string fname = stereoCameraSerialInfo_.filename;
+    std::string fname = stereoCameraSerialInfo_.left_camera_serial;
 
     stream = cv::VideoCapture(fname);
 

src/camera/widgets/loadstereoimagepairdialog.cpp

@@ -0,0 +1,43 @@
+#include "loadstereoimagepairdialog.h"
+#include "ui_loadstereoimagepairdialog.h"
+
+LoadStereoImagePairDialog::LoadStereoImagePairDialog(QWidget *parent) :
+    QDialog(parent),
+    ui(new Ui::LoadStereoImagePairDialog)
+{
+    ui->setupUi(this);
+
+   connect(ui->btnBrowseLeft, SIGNAL(clicked()), this, SLOT(requestLeftImageFilepath()));
+   connect(ui->btnBrowseRight, SIGNAL(clicked()), this, SLOT(requestRightImageFilepath()));
+   connect(ui->btnCancel, SIGNAL(clicked()), this, SLOT(reject()));
+   connect(ui->btnLoad, SIGNAL(clicked()), this, SLOT(accept()));
+}
+
+void LoadStereoImagePairDialog::reject()
+{
+    leftFilepathValid = rightFilepathValid = false;
+    QDialog::reject();
+}
+
+void LoadStereoImagePairDialog::requestLeftImageFilepath(){
+    leftFilepathValid = requestImageFilepath(left_image_filepath);
+    ui->txtLeftImageFilename->setText(QString::fromStdString(left_image_filepath));
+}
+
+void LoadStereoImagePairDialog::requestRightImageFilepath(){
+    rightFilepathValid = requestImageFilepath(right_image_filepath);
+    ui->txtRightImageFilename->setText(QString::fromStdString(right_image_filepath));
+}
+
+bool LoadStereoImagePairDialog::requestImageFilepath(std::string &fname){
+    QString qfname = QFileDialog::getOpenFileName(
+                this, tr("Open image"), "", tr("Images (*.png *.jpeg *.jpg)"));
+    fname = qfname.toStdString();
+    QFileInfo check_file(qfname);
+    return (fname != "" && check_file.exists() && check_file.isFile());
+}
+
+LoadStereoImagePairDialog::~LoadStereoImagePairDialog()
+{
+    delete ui;
+}

src/camera/widgets/loadstereoimagepairdialog.h

@@ -0,0 +1,41 @@
+#ifndef LOADSTEREOIMAGEPAIRDIALOG_H
+#define LOADSTEREOIMAGEPAIRDIALOG_H
+
+#include <QDialog>
+#include <QFileDialog>
+#include <QFileInfo>
+
+namespace Ui {
+class LoadStereoImagePairDialog;
+}
+
+class LoadStereoImagePairDialog : public QDialog
+{
+    Q_OBJECT
+
+public:
+    explicit LoadStereoImagePairDialog(QWidget *parent = nullptr);
+    ~LoadStereoImagePairDialog();
+
+    bool isFilepathsValid(){return leftFilepathValid && rightFilepathValid;}
+
+    std::string getLeftImageFilepath(){return left_image_filepath;}
+    std::string getRightImageFilepath(){return right_image_filepath;}
+
+private slots:
+    void requestLeftImageFilepath();
+    void requestRightImageFilepath();
+    void reject();
+
+private:
+    Ui::LoadStereoImagePairDialog *ui;
+
+    std::string left_image_filepath = "";
+    std::string right_image_filepath = "";
+    bool leftFilepathValid = false;
+    bool rightFilepathValid = false;
+
+    bool requestImageFilepath(std::string &fname);
+};
+
+#endif // LOADSTEREOIMAGEPAIRDIALOG_H