Visual Servoing Platform  version 3.5.1 under development (2022-08-18)
vpOccipitalStructure.h
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2021 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * libStructure interface.
33  *
34  * Authors:
35  * Joudy Nader
36  *
37  *****************************************************************************/
38 
39 #ifndef _vpOccipitalStructure_h_
40 #define _vpOccipitalStructure_h_
41 
42 #include <visp3/core/vpConfig.h>
43 
44 #if defined(VISP_HAVE_OCCIPITAL_STRUCTURE) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
45 #include <condition_variable>
46 #include <mutex>
47 
48 #include <ST/CaptureSession.h>
49 
50 #ifdef VISP_HAVE_PCL
51 #include <pcl/common/common_headers.h>
52 #endif
53 
54 #include <visp3/core/vpCameraParameters.h>
55 #include <visp3/core/vpImage.h>
56 
200 #ifndef DOXYGEN_SHOULD_SKIP_THIS
201 struct SessionDelegate : ST::CaptureSessionDelegate {
202  std::mutex m_sampleLock;
203  std::condition_variable cv_sampleLock;
204 
205  ST::ColorFrame m_visibleFrame;
206  ST::DepthFrame m_depthFrame;
207  ST::InfraredFrame m_infraredFrame;
208  ST::AccelerometerEvent m_accelerometerEvent;
209  ST::GyroscopeEvent m_gyroscopeEvent;
210  ST::StructureCoreCameraType m_cameraType;
211  ST::CaptureSessionUSBVersion m_USBVersion;
212  std::string m_serialNumber;
213 
214  ~SessionDelegate() {}
215 
216  void captureSessionEventDidOccur(ST::CaptureSession *session, ST::CaptureSessionEventId event) override
217  {
218  switch (event) {
219  case ST::CaptureSessionEventId::Booting:
220  break;
221  case ST::CaptureSessionEventId::Connected:
222  printf("Starting streams...\n");
223  session->startStreaming();
224  // The following wait function will let the capture session load correctly.
225  vpTime::wait(1000);
226 
227  // Getting details about capture session.
228  // (USB Version, Serial Number of the camera connected, Camera Monochorme/Color)
229  m_USBVersion = session->USBVersion();
230  m_serialNumber = session->sensorInfo().serialNumber;
231  m_cameraType = session->getCameraType();
232  break;
233  case ST::CaptureSessionEventId::Disconnected:
234  break;
235  case ST::CaptureSessionEventId::Error:
236  throw vpException(vpException::fatalError, "Capture session error");
237  break;
238  default:
239  printf("Capture session event unhandled\n");
240  }
241  }
242 
243  void captureSessionDidOutputSample(ST::CaptureSession *, const ST::CaptureSessionSample &sample) override
244  {
245  // acquire sampleLock mutex.
246  std::lock_guard<std::mutex> u(m_sampleLock);
247 
248  // Perform the modification needed on the shared variables.
249  if (sample.visibleFrame.isValid())
250  m_visibleFrame = sample.visibleFrame;
251 
252  if (sample.depthFrame.isValid())
253  m_depthFrame = sample.depthFrame;
254 
255  if (sample.infraredFrame.isValid())
256  m_infraredFrame = sample.infraredFrame;
257 
258  if (sample.type == ST::CaptureSessionSample::Type::AccelerometerEvent)
259  m_accelerometerEvent = sample.accelerometerEvent;
260 
261  if (sample.type == ST::CaptureSessionSample::Type::GyroscopeEvent)
262  m_gyroscopeEvent = sample.gyroscopeEvent;
263 
264  // If any thread is waiting on `cv_sampleLock`, the following instruction will unblock it.
265  // In our case, `open()` and `acquire()` will be blocked on `cv_sampleLock`.
266  cv_sampleLock.notify_one();
267  }
268 };
269 #endif // DOXYGEN_SHOULD_SKIP_THIS
270 
271 class VISP_EXPORT vpOccipitalStructure
272 {
273 public:
274  typedef enum {
278  imu
280 
283 
284  void acquire(vpImage<unsigned char> &gray, bool undistorted = false, double *ts = NULL);
285  void acquire(vpImage<vpRGBa> &rgb, bool undistorted = false, double *ts = NULL);
286 
287  void acquire(vpImage<vpRGBa> *rgb, vpImage<vpRGBa> *depth, vpColVector *acceleration_data = NULL,
288  vpColVector *gyroscope_data = NULL, bool undistorted = false, double *ts = NULL);
289  void acquire(vpImage<unsigned char> *gray, vpImage<vpRGBa> *depth, vpColVector *acceleration_data = NULL,
290  vpColVector *gyroscope_data = NULL, bool undistorted = false, double *ts = NULL);
291 
292  void acquire(unsigned char *const data_image, unsigned char *const data_depth,
293  std::vector<vpColVector> *const data_pointCloud = NULL, unsigned char *const data_infrared = NULL,
294  vpColVector *acceleration_data = NULL, vpColVector *gyroscope_data = NULL, bool undistorted = true,
295  double *ts = NULL);
296 
297 #ifdef VISP_HAVE_PCL
298  void acquire(unsigned char *const data_image, unsigned char *const data_depth,
299  std::vector<vpColVector> *const data_pointCloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud,
300  unsigned char *const data_infrared = NULL, vpColVector *acceleration_data = NULL,
301  vpColVector *gyroscope_data = NULL, bool undistorted = true, double *ts = NULL);
302  void acquire(unsigned char *const data_image, unsigned char *const data_depth,
303  std::vector<vpColVector> *const data_pointCloud, pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud,
304  unsigned char *const data_infrared = NULL, vpColVector *acceleration_data = NULL,
305  vpColVector *gyroscope_data = NULL, bool undistorted = true, double *ts = NULL);
306 #endif
307 
308  void getIMUVelocity(vpColVector *imu_vel, double *ts);
309  void getIMUAcceleration(vpColVector *imu_acc, double *ts);
310  void getIMUData(vpColVector *imu_vel, vpColVector *imu_acc, double *ts = NULL);
311 
312  bool open(const ST::CaptureSessionSettings &settings);
313  void close();
314 
318  ST::StructureCoreCameraType getCameraType() const { return m_delegate.m_cameraType; }
319 
320  ST::CaptureSessionUSBVersion getUSBVersion() const { return m_delegate.m_USBVersion; }
321  std::string getSerialNumber() const { return m_delegate.m_serialNumber; }
322  ST::CaptureSession &getCaptureSession() { return m_captureSession; }
323  ST::CaptureSessionSettings &getCaptureSessionSettings() { return m_captureSessionSettings; }
324 
325  unsigned int getWidth(vpOccipitalStructureStream stream_type);
326  unsigned int getHeight(vpOccipitalStructureStream stream_type);
327 
328  // Returns depth in millimeters at (x,y) if it exists, NAN otherwise.
329  float getDepth(int x, int y);
330 
331  vpPoint unprojectPoint(int row, int col);
332 
333  vpHomogeneousMatrix getTransform(const vpOccipitalStructureStream from, const vpOccipitalStructureStream to);
334 
335  ST::Intrinsics getIntrinsics(const vpOccipitalStructureStream stream_type) const;
336 
337  vpCameraParameters getCameraParameters(
338  const vpOccipitalStructureStream stream_type,
340 
341  void saveDepthImageAsPointCloudMesh(std::string &filename);
342 
343 protected:
344  bool m_init;
346  float m_maxZ;
347 
348  ST::CaptureSession m_captureSession;
349  ST::CaptureSessionSettings m_captureSessionSettings;
350  SessionDelegate m_delegate;
352 
353  void getPointcloud(std::vector<vpColVector> &pointcloud);
354 #ifdef VISP_HAVE_PCL
355  void getPointcloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud);
356  void getColoredPointcloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud);
357 #endif
358 };
359 
360 #endif
361 #endif
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:174
ST::CaptureSession m_captureSession
vpCameraParameters m_visible_camera_parameters
Implementation of an homogeneous matrix and operations on such kind of matrices.
ST::StructureCoreCameraType getCameraType() const
error that can be emited by ViSP classes.
Definition: vpException.h:71
ST::CaptureSessionSettings m_captureSessionSettings
std::string getSerialNumber() const
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:81
ST::CaptureSessionUSBVersion getUSBVersion() const
Generic class defining intrinsic camera parameters.
ST::CaptureSessionSettings & getCaptureSessionSettings()
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
ST::CaptureSession & getCaptureSession()