vpRealSense.cpp

/****************************************************************************
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2017 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:
 * RealSense SDK wrapper.
 *
 * Authors:
 * Fabien Spindler
 *
 *****************************************************************************/

#include <iomanip>
#include <iostream>

#include <visp3/core/vpImageConvert.h>
#include <visp3/sensor/vpRealSense.h>

#if defined(VISP_HAVE_REALSENSE) && defined(VISP_HAVE_CPP11_COMPATIBILITY)

#include "vpRealSense_impl.h"

vpRealSense::vpRealSense()
  : m_context(), m_device(NULL), m_num_devices(0), m_serial_no(), m_intrinsics(), m_max_Z(8), m_enableStreams(),
    m_useStreamPresets(), m_streamPresets(), m_streamParams(), m_invalidDepthValue(0.0f)
{
  initStream();
}

vpRealSense::~vpRealSense() { close(); }

void vpRealSense::initStream()
{
  // General default presets
  // Color stream
  m_useStreamPresets[rs::stream::color] = true;
  m_streamPresets[rs::stream::color] = rs::preset::best_quality;
  m_streamParams[rs::stream::color] = vpRsStreamParams(640, 480, rs::format::rgba8, 60);

  // Depth stream
  m_useStreamPresets[rs::stream::depth] = true;
  m_streamPresets[rs::stream::depth] = rs::preset::best_quality;
  m_streamParams[rs::stream::depth] = vpRsStreamParams(640, 480, rs::format::z16, 60);

  // Infrared stream
  m_useStreamPresets[rs::stream::infrared] = true;
  m_streamPresets[rs::stream::infrared] = rs::preset::best_quality;
  m_streamParams[rs::stream::infrared] = vpRsStreamParams(640, 480, rs::format::y16, 60);

  // Infrared stream 2
  m_useStreamPresets[rs::stream::infrared2] = true;
  m_streamPresets[rs::stream::infrared2] = rs::preset::best_quality;
  m_streamParams[rs::stream::infrared2] = vpRsStreamParams(640, 480, rs::format::y16, 60);

  // Enable all streams
  m_enableStreams[rs::stream::color] = true;
  m_enableStreams[rs::stream::depth] = true;
  m_enableStreams[rs::stream::infrared] = true;
  m_enableStreams[rs::stream::infrared2] = true;
}

void vpRealSense::open()
{
  m_num_devices = m_context.get_device_count();

  if (m_num_devices == 0)
    throw vpException(vpException::fatalError, "RealSense Camera - No device detected.");

  std::vector<rs::device *> detected_devices;
  rs::device *device;
  for (int i = 0; i < m_num_devices; ++i) {
    device = m_context.get_device(i);
    std::string serial_no = device->get_serial();
    if (serial_no.compare(m_serial_no) == 0) {
      m_device = device;
    }
    detected_devices.push_back(device);
  }

  // Exit with error if no serial number is specified and multiple cameras are
  // detected.
  if ((m_serial_no.empty()) && (m_num_devices > 1)) {
    throw vpException(vpException::fatalError,
                      "RealSense Camera - Multiple cameras detected (%d) but "
                      "no input serial number specified. Exiting!",
                      m_num_devices);
  }
  // Exit with error if no camera is detected that matches the input serial
  // number.
  if ((!m_serial_no.empty()) && (m_device == NULL)) {
    throw vpException(vpException::fatalError,
                      "RealSense Camera - No camera detected with input "
                      "serial_no \"%s\" Exiting!",
                      m_serial_no.c_str());
  }

  // At this point, m_device will be null if no input serial number was
  // specified and only one camera is connected. This is a valid use case and
  // the code will proceed.
  m_device = m_context.get_device(0);

  std::cout << "RealSense Camera - Connecting to camera with Serial No: " << m_device->get_serial() << std::endl;

  // Enable only infrared2 stream if supported
  m_enableStreams[rs::stream::infrared2] = m_enableStreams[rs::stream::infrared2]
                                               ? m_device->supports(rs::capabilities::infrared2)
                                               : m_enableStreams[rs::stream::infrared2];

  if (m_device->is_streaming()) {
    m_device->stop();
  }

  for (int j = 0; j < 4; j++) {
    auto s = (rs::stream)j;
    auto capabilities = (rs::capabilities)j;
    if (m_device->supports(capabilities) && m_device->is_stream_enabled(s)) {
      m_device->disable_stream(s);
    }
  }

  if (m_enableStreams[rs::stream::color]) {
    if (m_useStreamPresets[rs::stream::color]) {
      m_device->enable_stream(rs::stream::color, m_streamPresets[rs::stream::color]);
    } else {
      m_device->enable_stream(rs::stream::color, m_streamParams[rs::stream::color].m_streamWidth,
                              m_streamParams[rs::stream::color].m_streamHeight,
                              m_streamParams[rs::stream::color].m_streamFormat,
                              m_streamParams[rs::stream::color].m_streamFramerate);
    }
  }

  if (m_enableStreams[rs::stream::depth]) {
    if (m_useStreamPresets[rs::stream::depth]) {
      m_device->enable_stream(rs::stream::depth, m_streamPresets[rs::stream::depth]);
    } else {
      m_device->enable_stream(rs::stream::depth, m_streamParams[rs::stream::depth].m_streamWidth,
                              m_streamParams[rs::stream::depth].m_streamHeight,
                              m_streamParams[rs::stream::depth].m_streamFormat,
                              m_streamParams[rs::stream::depth].m_streamFramerate);
    }
  }

  if (m_enableStreams[rs::stream::infrared]) {
    if (m_useStreamPresets[rs::stream::infrared]) {
      m_device->enable_stream(rs::stream::infrared, m_streamPresets[rs::stream::infrared]);
    } else {
      m_device->enable_stream(rs::stream::infrared, m_streamParams[rs::stream::infrared].m_streamWidth,
                              m_streamParams[rs::stream::infrared].m_streamHeight,
                              m_streamParams[rs::stream::infrared].m_streamFormat,
                              m_streamParams[rs::stream::infrared].m_streamFramerate);
    }
  }

  if (m_enableStreams[rs::stream::infrared2]) {
    if (m_useStreamPresets[rs::stream::infrared2]) {
      m_device->enable_stream(rs::stream::infrared2, m_streamPresets[rs::stream::infrared2]);
    } else {
      m_device->enable_stream(rs::stream::infrared2, m_streamParams[rs::stream::infrared2].m_streamWidth,
                              m_streamParams[rs::stream::infrared2].m_streamHeight,
                              m_streamParams[rs::stream::infrared2].m_streamFormat,
                              m_streamParams[rs::stream::infrared2].m_streamFramerate);
    }
  }

  // Compute field of view for each enabled stream
  m_intrinsics.clear();
  for (int i = 0; i < 4; ++i) {
    auto stream = rs::stream(i);
    if (!m_device->is_stream_enabled(stream))
      continue;
    auto intrin = m_device->get_stream_intrinsics(stream);

    m_intrinsics[stream] = intrin;
  }

  // Add synthetic stream intrinsics
  if (m_enableStreams[rs::stream::color]) {
    m_intrinsics[rs::stream::rectified_color] = m_device->get_stream_intrinsics(rs::stream::rectified_color);

    if (m_enableStreams[rs::stream::depth]) {
      m_intrinsics[rs::stream::color_aligned_to_depth] =
          m_device->get_stream_intrinsics(rs::stream::color_aligned_to_depth);
      m_intrinsics[rs::stream::depth_aligned_to_color] =
          m_device->get_stream_intrinsics(rs::stream::depth_aligned_to_color);
      m_intrinsics[rs::stream::depth_aligned_to_rectified_color] =
          m_device->get_stream_intrinsics(rs::stream::depth_aligned_to_rectified_color);
    }
  }

  if (m_enableStreams[rs::stream::depth] && m_enableStreams[rs::stream::infrared2]) {
    m_intrinsics[rs::stream::depth_aligned_to_infrared2] =
        m_device->get_stream_intrinsics(rs::stream::depth_aligned_to_infrared2);
    m_intrinsics[rs::stream::infrared2_aligned_to_depth] =
        m_device->get_stream_intrinsics(rs::stream::infrared2_aligned_to_depth);
  }

  // Start device
  m_device->start();
}

void vpRealSense::close()
{
  if (m_device) {
    if (m_device->is_streaming()) {
      m_device->stop();
    }
    m_device = NULL;
  }
}

void vpRealSense::setDeviceBySerialNumber(const std::string &serial_no)
{
  if (m_serial_no.empty()) {
    throw vpException(vpException::fatalError,
                      "RealSense Camera - Multiple cameras detected (%d) but "
                      "no input serial number specified. Exiting!",
                      m_num_devices);
  }

  m_serial_no = serial_no;
}

vpCameraParameters vpRealSense::getCameraParameters(const rs::stream &stream,
                                                    vpCameraParameters::vpCameraParametersProjType type) const
{
  auto intrinsics = this->getIntrinsics(stream);

  vpCameraParameters cam;
  double u0 = intrinsics.ppx;
  double v0 = intrinsics.ppy;
  double px = intrinsics.fx;
  double py = intrinsics.fy;
  if (type == vpCameraParameters::perspectiveProjWithDistortion) {
    double kdu = intrinsics.coeffs[0];
    cam.initPersProjWithDistortion(px, py, u0, v0, -kdu, kdu);
  } else {
    cam.initPersProjWithoutDistortion(px, py, u0, v0);
  }
  return cam;
}

rs::intrinsics vpRealSense::getIntrinsics(const rs::stream &stream) const
{
  if (!m_device) {
    throw vpException(vpException::fatalError, "RealSense Camera - device handler is null. Cannot "
                                               "retrieve intrinsics. Exiting!");
  }
  if (!m_device->is_stream_enabled(stream)) {
    throw vpException(vpException::fatalError,
                      "RealSense Camera - stream (%d) is not enabled to "
                      "retrieve intrinsics. Exiting!",
                      (int)stream);
  }

  std::map<rs::stream, rs::intrinsics>::const_iterator it_intrin = m_intrinsics.find(stream);
  if (it_intrin != m_intrinsics.end()) {
    return it_intrin->second;
  }

  return rs::intrinsics();
}

rs::extrinsics vpRealSense::getExtrinsics(const rs::stream &from, const rs::stream &to) const
{
  if (!m_device) {
    throw vpException(vpException::fatalError, "RealSense Camera - device handler is null. Cannot "
                                               "retrieve extrinsics. Exiting!");
  }
  if (!m_device->is_stream_enabled(from)) {
    throw vpException(vpException::fatalError,
                      "RealSense Camera - stream (%d) is not enabled to "
                      "retrieve extrinsics. Exiting!",
                      (int)from);
  }
  if (!m_device->is_stream_enabled(to)) {
    throw vpException(vpException::fatalError,
                      "RealSense Camera - stream (%d) is not enabled to "
                      "retrieve extrinsics. Exiting!",
                      (int)to);
  }
  return m_device->get_extrinsics(from, to);
}

vpHomogeneousMatrix vpRealSense::getTransformation(const rs::stream &from, const rs::stream &to) const
{
  rs::extrinsics extrinsics = this->getExtrinsics(from, to);
  vpTranslationVector t;
  vpRotationMatrix R;
  for (unsigned int i = 0; i < 3; i++) {
    t[i] = extrinsics.translation[i];
    for (unsigned int j = 0; j < 3; j++)
      R[i][j] = extrinsics.rotation[i * 3 + j];
  }
  vpHomogeneousMatrix fMt(t, R);
  return fMt;
}

void vpRealSense::acquire(vpImage<unsigned char> &grey)
{
  if (m_device == NULL) {
    throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
  }
  if (!m_device->is_streaming()) {
    open();
  }

  m_device->wait_for_frames();

  // Retrieve grey image
  vp_rs_get_grey_impl(m_device, m_intrinsics, grey);
}

void vpRealSense::acquire(vpImage<unsigned char> &grey, std::vector<vpColVector> &pointcloud)
{
  if (m_device == NULL) {
    throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
  }
  if (!m_device->is_streaming()) {
    open();
  }

  m_device->wait_for_frames();

  // Retrieve grey image
  vp_rs_get_grey_impl(m_device, m_intrinsics, grey);

  // Retrieve point cloud
  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
}

void vpRealSense::acquire(std::vector<vpColVector> &pointcloud)
{
  if (m_device == NULL) {
    throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
  }
  if (!m_device->is_streaming()) {
    open();
  }

  m_device->wait_for_frames();

  // Retrieve point cloud
  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
}

void vpRealSense::acquire(vpImage<vpRGBa> &color)
{
  if (m_device == NULL) {
    throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
  }
  if (!m_device->is_streaming()) {
    open();
  }

  m_device->wait_for_frames();

  // Retrieve color image
  vp_rs_get_color_impl(m_device, m_intrinsics, color);
}

void vpRealSense::acquire(vpImage<vpRGBa> &color, vpImage<uint16_t> &infrared, vpImage<uint16_t> &depth,
                          std::vector<vpColVector> &pointcloud)
{
  if (m_device == NULL) {
    throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
  }
  if (!m_device->is_streaming()) {
    open();
  }

  m_device->wait_for_frames();

  // Retrieve color image
  vp_rs_get_color_impl(m_device, m_intrinsics, color);

  // Retrieve infrared image
  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, infrared);

  // Retrieve depth image
  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, depth);

  // Retrieve point cloud
  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
}

void vpRealSense::acquire(vpImage<unsigned char> &grey, vpImage<uint16_t> &infrared, vpImage<uint16_t> &depth,
                          std::vector<vpColVector> &pointcloud)
{
  if (m_device == NULL) {
    throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
  }
  if (!m_device->is_streaming()) {
    open();
  }

  m_device->wait_for_frames();

  // Retrieve grey image
  vp_rs_get_grey_impl(m_device, m_intrinsics, grey);

  // Retrieve infrared image
  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, infrared);

  // Retrieve depth image
  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, depth);

  // Retrieve point cloud
  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
}

void vpRealSense::acquire(vpImage<vpRGBa> &color, std::vector<vpColVector> &pointcloud)
{
  if (m_device == NULL) {
    throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
  }
  if (!m_device->is_streaming()) {
    open();
  }

  m_device->wait_for_frames();

  // Retrieve color image
  vp_rs_get_color_impl(m_device, m_intrinsics, color);

  // Retrieve point cloud
  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
}

void vpRealSense::acquire(unsigned char *const data_image, unsigned char *const data_depth,
                          std::vector<vpColVector> *const data_pointCloud, unsigned char *const data_infrared,
                          unsigned char *const data_infrared2, const rs::stream &stream_color,
                          const rs::stream &stream_depth, const rs::stream &stream_infrared,
                          const rs::stream &stream_infrared2)
{
  if (m_device == NULL) {
    throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
  }

  if (!m_device->is_streaming()) {
    open();
  }

  m_device->wait_for_frames();

  if (data_image != NULL) {
    vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::color, data_image, stream_color);
  }

  if (data_depth != NULL) {
    vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, data_depth, stream_depth);

    if (data_pointCloud != NULL) {
      vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, *data_pointCloud, m_invalidDepthValue, stream_depth);
    }
  }

  if (data_infrared != NULL) {
    vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, data_infrared, stream_infrared);
  }

  if (data_infrared2 != NULL) {
    vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared2, data_infrared2, stream_infrared2);
  }
}

void vpRealSense::setStreamSettings(const rs::stream &stream, const rs::preset &preset)
{
  m_useStreamPresets[stream] = true;
  m_streamPresets[stream] = preset;
}

void vpRealSense::setStreamSettings(const rs::stream &stream, const vpRsStreamParams &params)
{
  m_useStreamPresets[stream] = false;
  m_streamParams[stream] = params;
}

void vpRealSense::setEnableStream(const rs::stream &stream, const bool status) { m_enableStreams[stream] = status; }

#ifdef VISP_HAVE_PCL

void vpRealSense::acquire(pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
{
  if (m_device == NULL) {
    throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
  }
  if (!m_device->is_streaming()) {
    open();
  }

  m_device->wait_for_frames();

  // Retrieve point cloud
  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
}

void vpRealSense::acquire(pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud)
{
  if (m_device == NULL) {
    throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
  }
  if (!m_device->is_streaming()) {
    open();
  }

  m_device->wait_for_frames();

  // Retrieve point cloud
  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
}

void vpRealSense::acquire(vpImage<unsigned char> &grey, pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
{
  if (m_device == NULL) {
    throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
  }
  if (!m_device->is_streaming()) {
    open();
  }

  m_device->wait_for_frames();

  // Retrieve grey image
  vp_rs_get_grey_impl(m_device, m_intrinsics, grey);

  // Retrieve point cloud
  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
}

void vpRealSense::acquire(vpImage<vpRGBa> &color, pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
{
  if (m_device == NULL) {
    throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
  }
  if (!m_device->is_streaming()) {
    open();
  }

  m_device->wait_for_frames();

  // Retrieve color image
  vp_rs_get_color_impl(m_device, m_intrinsics, color);

  // Retrieve point cloud
  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
}

void vpRealSense::acquire(vpImage<vpRGBa> &color, vpImage<uint16_t> &infrared, vpImage<uint16_t> &depth,
                          pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
{
  if (m_device == NULL) {
    throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
  }
  if (!m_device->is_streaming()) {
    open();
  }

  m_device->wait_for_frames();

  // Retrieve color image
  vp_rs_get_color_impl(m_device, m_intrinsics, color);

  // Retrieve infrared image
  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, infrared);

  // Retrieve depth image
  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, depth);

  // Retrieve point cloud
  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
}

void vpRealSense::acquire(vpImage<unsigned char> &grey, vpImage<uint16_t> &infrared, vpImage<uint16_t> &depth,
                          pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
{
  if (m_device == NULL) {
    throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
  }
  if (!m_device->is_streaming()) {
    open();
  }

  m_device->wait_for_frames();

  // Retrieve grey image
  vp_rs_get_grey_impl(m_device, m_intrinsics, grey);

  // Retrieve infrared image
  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, infrared);

  // Retrieve depth image
  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, depth);

  // Retrieve point cloud
  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
}

void vpRealSense::acquire(vpImage<vpRGBa> &color, vpImage<uint16_t> &infrared, vpImage<uint16_t> &depth,
                          pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud)
{
  if (m_device == NULL) {
    throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
  }
  if (!m_device->is_streaming()) {
    open();
  }

  m_device->wait_for_frames();

  // Retrieve color image
  vp_rs_get_color_impl(m_device, m_intrinsics, color);

  // Retrieve infrared image
  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, infrared);

  // Retrieve depth image
  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, depth);

  // Retrieve point cloud
  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
}

void vpRealSense::acquire(vpImage<unsigned char> &grey, vpImage<uint16_t> &infrared, vpImage<uint16_t> &depth,
                          pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud)
{
  if (m_device == NULL) {
    throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
  }
  if (!m_device->is_streaming()) {
    open();
  }

  m_device->wait_for_frames();

  // Retrieve grey image
  vp_rs_get_grey_impl(m_device, m_intrinsics, grey);

  // Retrieve infrared image
  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, infrared);

  // Retrieve depth image
  vp_rs_get_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, depth);

  // Retrieve point cloud
  vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue);
}

void vpRealSense::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,
                          unsigned char *const data_infrared2, const rs::stream &stream_color,
                          const rs::stream &stream_depth, const rs::stream &stream_infrared,
                          const rs::stream &stream_infrared2)
{
  if (m_device == NULL) {
    throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
  }

  if (!m_device->is_streaming()) {
    open();
  }

  m_device->wait_for_frames();

  if (data_image != NULL) {
    vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::color, data_image, stream_color);
  }

  if (data_depth != NULL) {
    vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, data_depth, stream_depth);
  }

  if (data_pointCloud != NULL) {
    vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, *data_pointCloud, m_invalidDepthValue, stream_depth);
  }

  if (pointcloud != NULL) {
    vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue, stream_depth);
  }

  if (data_infrared != NULL) {
    vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, data_infrared, stream_infrared);
  }

  if (data_infrared2 != NULL) {
    vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared2, data_infrared2, stream_infrared2);
  }
}

void vpRealSense::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,
                          unsigned char *const data_infrared2, const rs::stream &stream_color,
                          const rs::stream &stream_depth, const rs::stream &stream_infrared,
                          const rs::stream &stream_infrared2)
{
  if (m_device == NULL) {
    throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
  }

  if (!m_device->is_streaming()) {
    open();
  }

  m_device->wait_for_frames();

  if (data_image != NULL) {
    vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::color, data_image, stream_color);
  }

  if (data_depth != NULL) {
    vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::depth, data_depth, stream_depth);
  }

  if (data_pointCloud != NULL) {
    vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, *data_pointCloud, m_invalidDepthValue, stream_depth);
  }

  if (pointcloud != NULL) {
    vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue, stream_color,
                              stream_depth);
  }

  if (data_infrared != NULL) {
    vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared, data_infrared, stream_infrared);
  }

  if (data_infrared2 != NULL) {
    vp_rs_get_native_frame_data_impl(m_device, m_intrinsics, rs::stream::infrared2, data_infrared2, stream_infrared2);
  }
}
#endif

std::ostream &operator<<(std::ostream &os, const vpRealSense &rs)
{
  os << "Device name: " << rs.getHandler()->get_name() << std::endl;
  std::streamsize ss = std::cout.precision();
  for (int i = 0; i < 4; ++i) {
    auto stream = rs::stream(i);
    if (!rs.getHandler()->is_stream_enabled(stream))
      continue;
    auto intrin = rs.getHandler()->get_stream_intrinsics(stream);
    std::cout << "Capturing " << stream << " at " << intrin.width << " x " << intrin.height;
    std::cout << std::setprecision(1) << std::fixed << ", fov = " << intrin.hfov() << " x " << intrin.vfov()
              << ", distortion = " << intrin.model() << std::endl;
  }
  std::cout.precision(ss);

  return os;
}

#elif !defined(VISP_BUILD_SHARED_LIBS)
// Work arround to avoid warning: libvisp_sensor.a(vpRealSense.cpp.o) has no
// symbols
void dummy_vpRealSense(){};
#endif