vpOccipitalStructure.h

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2021 by Inria. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact Inria about acquiring a ViSP Professional
 * Edition License.
 *
 * See http://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * If you have questions regarding the use of this file, please contact
 * Inria at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 * Description:
 * libStructure interface.
 *
 * Authors:
 * Joudy Nader
 *
 *****************************************************************************/

#ifndef _vpOccipitalStructure_h_
#define _vpOccipitalStructure_h_

#include <visp3/core/vpConfig.h>

#if defined(VISP_HAVE_OCCIPITAL_STRUCTURE) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
#include <mutex>
#include <condition_variable>

#include <ST/CaptureSession.h>

#ifdef VISP_HAVE_PCL
#include <pcl/common/common_headers.h>
#endif

#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpImage.h>

struct SessionDelegate : ST::CaptureSessionDelegate {
  std::mutex m_sampleLock;
  std::condition_variable cv_sampleLock;

  ST::ColorFrame m_visibleFrame;
  ST::DepthFrame m_depthFrame;
  ST::InfraredFrame m_infraredFrame;
  ST::AccelerometerEvent m_accelerometerEvent;
  ST::GyroscopeEvent m_gyroscopeEvent;
  ST::StructureCoreCameraType m_cameraType;
  ST::CaptureSessionUSBVersion m_USBVersion;
  std::string m_serialNumber;

  ~SessionDelegate() {}

  void captureSessionEventDidOccur(ST::CaptureSession *session, ST::CaptureSessionEventId event) override {
    switch (event) {
      case ST::CaptureSessionEventId::Booting: break;
      case ST::CaptureSessionEventId::Connected:
        printf("Starting streams...\n");
        session->startStreaming();
        // The following wait function will let the capture session load correctly.
        vpTime::wait(1000);

        // Getting details about capture session.
        // (USB Version, Serial Number of the camera connected, Camera Monochorme/Color)
        m_USBVersion = session->USBVersion();
        m_serialNumber = session->sensorInfo().serialNumber;
        m_cameraType = session->getCameraType();
        break;
      case ST::CaptureSessionEventId::Disconnected:
        break;
      case ST::CaptureSessionEventId::Error:
        throw vpException(vpException::fatalError, "Capture session error");
        break;
      default:
        printf("Capture session event unhandled\n");
    }
  }

  void captureSessionDidOutputSample(ST::CaptureSession *, const ST::CaptureSessionSample& sample) override {
    // acquire sampleLock mutex.
    std::lock_guard<std::mutex> u(m_sampleLock);

    // Perform the modification needed on the shared variables.
    if(sample.visibleFrame.isValid())
      m_visibleFrame = sample.visibleFrame;

    if(sample.depthFrame.isValid())
      m_depthFrame = sample.depthFrame;

    if(sample.infraredFrame.isValid())
      m_infraredFrame = sample.infraredFrame;

    if(sample.type == ST::CaptureSessionSample::Type::AccelerometerEvent)
      m_accelerometerEvent = sample.accelerometerEvent;

    if(sample.type == ST::CaptureSessionSample::Type::GyroscopeEvent)
      m_gyroscopeEvent = sample.gyroscopeEvent;

    // If any thread is waiting on `cv_sampleLock`, the following instruction will unblock it.
    // In our case, `open()` and `acquire()` will be blocked on `cv_sampleLock`.
    cv_sampleLock.notify_one();
  }
};

class VISP_EXPORT vpOccipitalStructure
{
  public:
    typedef enum
    {
      visible,       
      depth,         
      infrared,      
      imu            
    } vpOccipitalStructureStream;

    vpOccipitalStructure();
    ~vpOccipitalStructure();

    void acquire(vpImage<unsigned char> &gray, bool undistorted=false, double *ts=NULL);
    void acquire(vpImage<vpRGBa> &rgb, bool undistorted=false, double *ts=NULL);

    void acquire(vpImage<vpRGBa> *rgb, vpImage<vpRGBa> *depth, vpColVector *acceleration_data=NULL, vpColVector *gyroscope_data=NULL,
                 bool undistorted=false, double *ts=NULL);
    void acquire(vpImage<unsigned char> *gray, vpImage<vpRGBa> *depth, vpColVector *acceleration_data=NULL, vpColVector *gyroscope_data=NULL,
                 bool undistorted=false, double *ts=NULL);

    void acquire(unsigned char *const data_image, unsigned char *const data_depth,
                 std::vector<vpColVector> *const data_pointCloud=NULL, unsigned char *const data_infrared=NULL,
                 vpColVector *acceleration_data=NULL, vpColVector *gyroscope_data=NULL,
                 bool undistorted=true, double *ts=NULL);

    #ifdef VISP_HAVE_PCL
    void acquire(unsigned char *const data_image, unsigned char *const data_depth,
                 std::vector<vpColVector> *const data_pointCloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud,
                 unsigned char *const data_infrared=NULL, vpColVector *acceleration_data=NULL, vpColVector *gyroscope_data=NULL,
                 bool undistorted=true,double *ts=NULL);
    void acquire(unsigned char *const data_image, unsigned char *const data_depth,
                 std::vector<vpColVector> *const data_pointCloud, pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud,
                 unsigned char *const data_infrared=NULL, vpColVector *acceleration_data=NULL, vpColVector *gyroscope_data=NULL,
                 bool undistorted=true,double *ts=NULL);
    #endif

    void getIMUVelocity(vpColVector *imu_vel, double *ts);
    void getIMUAcceleration(vpColVector *imu_acc, double *ts);
    void getIMUData(vpColVector *imu_vel, vpColVector *imu_acc, double *ts=NULL);

    bool open(const ST::CaptureSessionSettings &settings);
    void close();

    ST::StructureCoreCameraType getCameraType() const { return m_delegate.m_cameraType; }

    ST::CaptureSessionUSBVersion getUSBVersion() const { return m_delegate.m_USBVersion; }
    std::string getSerialNumber() const { return m_delegate.m_serialNumber; }
    ST::CaptureSession &getCaptureSession() { return m_captureSession; }
    ST::CaptureSessionSettings &getCaptureSessionSettings() { return m_captureSessionSettings; }

    unsigned int getWidth(vpOccipitalStructureStream stream_type);
    unsigned int getHeight(vpOccipitalStructureStream stream_type);

    // Returns depth in millimeters at (x,y) if it exists, NAN otherwise.
    float getDepth(int x, int y);

    vpPoint unprojectPoint(int row, int col);

    vpHomogeneousMatrix getTransform(const vpOccipitalStructureStream from, const vpOccipitalStructureStream to);

    ST::Intrinsics getIntrinsics(const vpOccipitalStructureStream stream_type) const;

    vpCameraParameters getCameraParameters(const vpOccipitalStructureStream stream_type,
                                           vpCameraParameters::vpCameraParametersProjType type = vpCameraParameters::perspectiveProjWithoutDistortion);

    void saveDepthImageAsPointCloudMesh(std::string &filename);

  protected:
    bool m_init;
    float m_invalidDepthValue;
    float m_maxZ;

    ST::CaptureSession m_captureSession;
    ST::CaptureSessionSettings m_captureSessionSettings;
    SessionDelegate m_delegate;
    vpCameraParameters m_visible_camera_parameters, m_depth_camera_parameters;

    void getPointcloud(std::vector<vpColVector> &pointcloud);
    #ifdef VISP_HAVE_PCL
    void getPointcloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud);
    void getColoredPointcloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud);
    #endif
};

#endif
#endif