vpFlyCaptureGrabber.cpp

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2019 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: Class which enables to project an image in the 3D space
 * and get the view of a virtual camera.
 *
 * Authors:
 * Fabien Spindler
 *
 *****************************************************************************/

#include <visp3/core/vpException.h>
#include <visp3/sensor/vpFlyCaptureGrabber.h>

#ifdef VISP_HAVE_FLYCAPTURE

#include <visp3/core/vpTime.h>

vpFlyCaptureGrabber::vpFlyCaptureGrabber()
  : m_camera(), m_guid(), m_index(0), m_numCameras(0), m_rawImage(), m_connected(false), m_capture(false)
{
  m_numCameras = this->getNumCameras();
}

vpFlyCaptureGrabber::~vpFlyCaptureGrabber() { close(); }
unsigned int vpFlyCaptureGrabber::getNumCameras()
{
  unsigned int numCameras;
  FlyCapture2::BusManager busMgr;
  FlyCapture2::Error error = busMgr.GetNumOfCameras(&numCameras);
  if (error != FlyCapture2::PGRERROR_OK) {
    numCameras = 0;
  }
  return numCameras;
}

std::ostream &vpFlyCaptureGrabber::getCameraInfo(std::ostream &os)
{
  this->connect();

  FlyCapture2::CameraInfo camInfo;
  FlyCapture2::Error error = m_camera.GetCameraInfo(&camInfo);
  if (error != FlyCapture2::PGRERROR_OK) {
    error.PrintErrorTrace();
  }

  os << "Camera information:   " << std::endl;
  os << " Serial number      : " << camInfo.serialNumber << std::endl;
  os << " Camera model       : " << camInfo.modelName << std::endl;
  os << " Camera vendor      : " << camInfo.vendorName << std::endl;
  os << " Sensor             : " << camInfo.sensorInfo << std::endl;
  os << " Resolution         : " << camInfo.sensorResolution << std::endl;
  os << " Firmware version   : " << camInfo.firmwareVersion << std::endl;
  os << " Firmware build time: " << camInfo.firmwareBuildTime << std::endl;
  return os;
}

FlyCapture2::Camera *vpFlyCaptureGrabber::getCameraHandler()
{
  this->connect();

  if (m_connected == true) {
    return &m_camera;
  } else {
    return NULL;
  }
}

float vpFlyCaptureGrabber::getFrameRate()
{
  this->connect();

  FlyCapture2::Property prop = this->getProperty(FlyCapture2::FRAME_RATE);
  return prop.absValue;
}

float vpFlyCaptureGrabber::getShutter()
{
  this->connect();

  FlyCapture2::Property prop = this->getProperty(FlyCapture2::SHUTTER);
  return prop.absValue;
}

float vpFlyCaptureGrabber::getGain()
{
  this->connect();

  FlyCapture2::Property prop = this->getProperty(FlyCapture2::GAIN);
  return prop.absValue;
}

float vpFlyCaptureGrabber::getBrightness()
{
  this->connect();

  FlyCapture2::Property prop = this->getProperty(FlyCapture2::BRIGHTNESS);
  return prop.absValue;
}

unsigned int vpFlyCaptureGrabber::getSharpness()
{
  this->connect();

  FlyCapture2::Property prop = this->getProperty(FlyCapture2::SHARPNESS);
  return prop.valueA;
}

float vpFlyCaptureGrabber::getExposure()
{
  this->connect();

  FlyCapture2::Property prop = this->getProperty(FlyCapture2::AUTO_EXPOSURE);
  return prop.absValue;
}

unsigned int vpFlyCaptureGrabber::getCameraSerial(unsigned int index)
{
  unsigned int num_cameras = vpFlyCaptureGrabber::getNumCameras();
  if (index >= num_cameras) {
    throw(vpException(vpException::badValue, "The camera with index %u is not present. Only %d cameras connected.",
                      index, num_cameras));
  }
  unsigned int serial_id;
  FlyCapture2::BusManager busMgr;
  FlyCapture2::Error error;
  error = busMgr.GetCameraSerialNumberFromIndex(index, &serial_id);
  if (error != FlyCapture2::PGRERROR_OK) {
    error.PrintErrorTrace();
    throw(vpException(vpException::fatalError, "Cannot get camera with index %d serial id.", index));
  }
  return serial_id;
}

void vpFlyCaptureGrabber::setCameraIndex(unsigned int index)
{
  if (index >= m_numCameras) {
    throw(vpException(vpException::badValue, "The camera with index %u is not present. Only %d cameras connected.",
                      index, m_numCameras));
  }

  m_index = index;
}

void vpFlyCaptureGrabber::setCameraSerial(unsigned int serial_id)
{
  FlyCapture2::BusManager busMgr;
  FlyCapture2::Error error;
  m_numCameras = this->getNumCameras();
  for (unsigned int i = 0; i < m_numCameras; i++) {
    if (vpFlyCaptureGrabber::getCameraSerial(i) == serial_id) {
      m_index = i;
      return;
    }
  }
  throw(vpException(vpException::badValue, "The camera with serial id %u is not present.", serial_id));
}

void vpFlyCaptureGrabber::setProperty(const FlyCapture2::PropertyType &prop_type, bool on, bool auto_on, float value,
                                      PropertyValue prop_value)
{
  this->connect();

  FlyCapture2::PropertyInfo propInfo;
  propInfo = this->getPropertyInfo(prop_type);

  if (propInfo.present) {
    FlyCapture2::Property prop;
    prop.type = prop_type;
    prop.onOff = on && propInfo.onOffSupported;
    prop.autoManualMode = auto_on && propInfo.autoSupported;
    prop.absControl = propInfo.absValSupported;
    switch (prop_value) {
    case ABS_VALUE: {
      float value_ = (std::max)((std::min)((float)value, (float)propInfo.absMax), (float)propInfo.absMin);
      prop.absValue = value_;
      break;
    }
    case VALUE_A: {
      unsigned int value_ =
          (std::max)((std::min)((unsigned int)value, (unsigned int)propInfo.max), (unsigned int)propInfo.min);
      prop.valueA = value_;
      break;
    }
    }

    FlyCapture2::Error error;
    error = m_camera.SetProperty(&prop);
    if (error != FlyCapture2::PGRERROR_OK) {
      error.PrintErrorTrace();
      throw(vpException(vpException::fatalError, "Cannot set property %d.", (int)prop_type));
    }
  }
}

float vpFlyCaptureGrabber::setFrameRate(float frame_rate)
{
  this->connect();

  this->setProperty(FlyCapture2::FRAME_RATE, true, false, frame_rate);
  FlyCapture2::Property prop = this->getProperty(FlyCapture2::FRAME_RATE);
  return prop.absValue;
}

float vpFlyCaptureGrabber::setShutter(bool auto_shutter, float shutter_ms)
{
  this->connect();

  this->setProperty(FlyCapture2::SHUTTER, true, auto_shutter, shutter_ms);
  FlyCapture2::Property prop = this->getProperty(FlyCapture2::SHUTTER);
  return prop.absValue;
}

float vpFlyCaptureGrabber::setGain(bool gain_auto, float gain_value)
{
  this->connect();

  this->setProperty(FlyCapture2::GAIN, true, gain_auto, gain_value);
  FlyCapture2::Property prop = this->getProperty(FlyCapture2::GAIN);
  return prop.absValue;
}

float vpFlyCaptureGrabber::setBrightness(bool brightness_auto, float brightness_value)
{
  this->connect();

  this->setProperty(FlyCapture2::BRIGHTNESS, true, brightness_auto, brightness_value);
  FlyCapture2::Property prop = this->getProperty(FlyCapture2::BRIGHTNESS);
  return prop.absValue;
}

float vpFlyCaptureGrabber::setExposure(bool exposure_on, bool exposure_auto, float exposure_value)
{
  this->connect();

  this->setProperty(FlyCapture2::AUTO_EXPOSURE, exposure_on, exposure_auto, exposure_value);
  FlyCapture2::Property prop = this->getProperty(FlyCapture2::AUTO_EXPOSURE);
  return prop.absValue;
}

unsigned int vpFlyCaptureGrabber::setSharpness(bool sharpness_on, bool sharpness_auto, unsigned int sharpness_value)
{
  this->connect();

  this->setProperty(FlyCapture2::SHARPNESS, sharpness_on, sharpness_auto, (float)sharpness_value, VALUE_A);
  FlyCapture2::Property prop = this->getProperty(FlyCapture2::SHARPNESS);
  return prop.valueA;
}

FlyCapture2::Property vpFlyCaptureGrabber::getProperty(FlyCapture2::PropertyType prop_type)
{
  this->connect();

  FlyCapture2::Property prop;
  prop.type = prop_type;
  FlyCapture2::Error error;
  error = m_camera.GetProperty(&prop);
  if (error != FlyCapture2::PGRERROR_OK) {
    error.PrintErrorTrace();
    throw(vpException(vpException::fatalError, "Cannot get property %d value.", (int)prop_type));
  }
  return prop;
}

FlyCapture2::PropertyInfo vpFlyCaptureGrabber::getPropertyInfo(FlyCapture2::PropertyType prop_type)
{
  this->connect();

  FlyCapture2::PropertyInfo propInfo;
  propInfo.type = prop_type;

  FlyCapture2::Error error;
  error = m_camera.GetPropertyInfo(&propInfo);
  if (error != FlyCapture2::PGRERROR_OK) {
    error.PrintErrorTrace();
    throw(vpException(vpException::fatalError, "Cannot get property %d info.", (int)prop_type));
  }
  return propInfo;
}

void vpFlyCaptureGrabber::setVideoModeAndFrameRate(FlyCapture2::VideoMode video_mode, FlyCapture2::FrameRate frame_rate)
{
  this->connect();

  FlyCapture2::Error error;
  error = m_camera.SetVideoModeAndFrameRate(video_mode, frame_rate);
  if (error != FlyCapture2::PGRERROR_OK) {
    error.PrintErrorTrace();
    throw(vpException(vpException::fatalError, "Cannot set video mode and framerate."));
  }
}

bool vpFlyCaptureGrabber::isVideoModeAndFrameRateSupported(FlyCapture2::VideoMode video_mode,
                                                           FlyCapture2::FrameRate frame_rate)
{
  this->connect();

  FlyCapture2::Error error;
  bool supported = false;
  error = m_camera.GetVideoModeAndFrameRateInfo(video_mode, frame_rate, &supported);
  if (error != FlyCapture2::PGRERROR_OK) {
    error.PrintErrorTrace();
    throw(vpException(vpException::fatalError, "Cannot get video mode and framerate."));
  }
  return supported;
}

std::pair<unsigned int, unsigned int> vpFlyCaptureGrabber::centerRoi(unsigned int size, unsigned int max_size,
                                                                     unsigned int step)
{
  if (size == 0 || size > max_size)
    size = max_size;
  // size should be a multiple of step
  size = size / step * step;
  const unsigned int offset = (max_size - size) / 2;
  // Return offset for centering roi
  return std::make_pair(size, offset);
}

void vpFlyCaptureGrabber::setFormat7VideoMode(FlyCapture2::Mode format7_mode, FlyCapture2::PixelFormat pixel_format,
                                              unsigned int w, unsigned int h)
{
  this->connect();

  FlyCapture2::Format7Info fmt7_info;
  bool fmt7_supported;
  FlyCapture2::Error error;

  fmt7_info.mode = format7_mode;
  error = m_camera.GetFormat7Info(&fmt7_info, &fmt7_supported);
  if (error != FlyCapture2::PGRERROR_OK) {
    error.PrintErrorTrace();
    throw(vpException(vpException::fatalError, "Cannot get format7 info."));
  }
  if (!fmt7_supported) {
    throw(vpException(vpException::fatalError, "Format7 mode %d not supported.", (int)format7_mode));
  }

  FlyCapture2::Format7ImageSettings fmt7_settings;
  fmt7_settings.mode = format7_mode;
  fmt7_settings.pixelFormat = pixel_format;
  // Set centered roi
  std::pair<unsigned int, unsigned int> roi_w = this->centerRoi(w, fmt7_info.maxWidth, fmt7_info.imageHStepSize);
  std::pair<unsigned int, unsigned int> roi_h = this->centerRoi(h, fmt7_info.maxHeight, fmt7_info.imageVStepSize);
  fmt7_settings.width = roi_w.first;
  fmt7_settings.offsetX = roi_w.second;
  fmt7_settings.height = roi_h.first;
  fmt7_settings.offsetY = roi_h.second;

  // Validate the settings
  FlyCapture2::Format7PacketInfo fmt7_packet_info;
  bool valid = false;
  error = m_camera.ValidateFormat7Settings(&fmt7_settings, &valid, &fmt7_packet_info);
  if (error != FlyCapture2::PGRERROR_OK) {
    error.PrintErrorTrace();
    throw(vpException(vpException::fatalError, "Cannot validate format7 settings."));
  }
  if (!valid) {
    throw(vpException(vpException::fatalError, "Format7 settings are not valid."));
  }
  error = m_camera.SetFormat7Configuration(&fmt7_settings, fmt7_packet_info.recommendedBytesPerPacket);
  if (error != FlyCapture2::PGRERROR_OK) {
    error.PrintErrorTrace();
    throw(vpException(vpException::fatalError, "Cannot set format7 settings."));
  }
}

bool vpFlyCaptureGrabber::isFormat7Supported(FlyCapture2::Mode format7_mode)
{
  this->connect();

  FlyCapture2::Format7Info fmt7_info;
  bool supported = false;
  FlyCapture2::Error error;

  fmt7_info.mode = format7_mode;
  error = m_camera.GetFormat7Info(&fmt7_info, &supported);
  if (error != FlyCapture2::PGRERROR_OK) {
    error.PrintErrorTrace();
    throw(vpException(vpException::fatalError, "Cannot get format7 info."));
  }

  return supported;
}
void vpFlyCaptureGrabber::startCapture()
{
  this->connect();

  if (m_capture == false) {

    FlyCapture2::Error error;
    error = m_camera.StartCapture();
    if (error != FlyCapture2::PGRERROR_OK) {
      error.PrintErrorTrace();
      throw(vpException(vpException::fatalError, "Cannot start capture for camera with serial %u",
                        getCameraSerial(m_index)));
    }
    m_capture = true;
  }
  if (m_connected && m_capture)
    init = true;
  else
    init = false;
}

void vpFlyCaptureGrabber::stopCapture()
{
  if (m_capture == true) {

    FlyCapture2::Error error;
    error = m_camera.StopCapture();
    if (error != FlyCapture2::PGRERROR_OK) {
      error.PrintErrorTrace();
      throw(vpException(vpException::fatalError, "Cannot stop capture."));
    }
    m_capture = false;
  }
  if (m_connected && m_capture)
    init = true;
  else
    init = false;
}

void vpFlyCaptureGrabber::connect()
{
  if (m_connected == false) {
    FlyCapture2::Error error;
    m_numCameras = this->getNumCameras();
    if (m_numCameras == 0) {
      throw(vpException(vpException::fatalError, "No camera found on the bus"));
    }

    FlyCapture2::BusManager busMgr;

    error = busMgr.GetCameraFromIndex(m_index, &m_guid);
    if (error != FlyCapture2::PGRERROR_OK) {
      error.PrintErrorTrace();
      throw(vpException(vpException::fatalError, "Cannot retrieve guid of camera with index %u.", m_index));
    }
    // Connect to a camera
    error = m_camera.Connect(&m_guid);
    if (error != FlyCapture2::PGRERROR_OK) {
      error.PrintErrorTrace();
      throw(vpException(vpException::fatalError, "Cannot connect to camera with serial %u", getCameraSerial(m_index)));
    }
    m_connected = true;
  }
  if (m_connected && m_capture)
    init = true;
  else
    init = false;
}

void vpFlyCaptureGrabber::disconnect()
{
  if (m_connected == true) {

    FlyCapture2::Error error;
    error = m_camera.Disconnect();
    if (error != FlyCapture2::PGRERROR_OK) {
      error.PrintErrorTrace();
      throw(vpException(vpException::fatalError, "Cannot stop capture."));
    }
    m_connected = false;
  }
  if (m_connected && m_capture)
    init = true;
  else
    init = false;
}

void vpFlyCaptureGrabber::close()
{
  this->stopCapture();
  this->disconnect();
}

void vpFlyCaptureGrabber::acquire(vpImage<unsigned char> &I)
{
  FlyCapture2::TimeStamp timestamp;
  this->acquire(I, timestamp);
}

void vpFlyCaptureGrabber::acquire(vpImage<unsigned char> &I, FlyCapture2::TimeStamp &timestamp)
{
  this->open();

  FlyCapture2::Error error;
  // Retrieve an image
  error = m_camera.RetrieveBuffer(&m_rawImage);
  if (error != FlyCapture2::PGRERROR_OK) {
    error.PrintErrorTrace();
    std::cerr << "Cannot retrieve image from camera with serial " << getCameraSerial(m_index) << std::endl;
  }
  timestamp = m_rawImage.GetTimeStamp();

  height = m_rawImage.GetRows();
  width = m_rawImage.GetCols();
  I.resize(height, width);

  // Create a converted image using a stride equals to `sizeof(unsigned
  // char) * width`, which makes sure there is no paddings or holes
  // between pixel data. And the convertedImage object is sharing the
  // same data buffer with vpImage object `I`.
  FlyCapture2::Image convertedImage(height, width, sizeof(unsigned char) * width, I.bitmap,
                                    sizeof(unsigned char) * I.getSize(), FlyCapture2::PIXEL_FORMAT_MONO8);

  // Convert the raw image
  error = m_rawImage.Convert(FlyCapture2::PIXEL_FORMAT_MONO8, &convertedImage);
  if (error != FlyCapture2::PGRERROR_OK) {
    error.PrintErrorTrace();
    throw(vpException(vpException::fatalError, "Cannot convert image from camera with serial %u",
                      getCameraSerial(m_index)));
  }
}

void vpFlyCaptureGrabber::acquire(vpImage<vpRGBa> &I)
{
  FlyCapture2::TimeStamp timestamp;
  this->acquire(I, timestamp);
}

void vpFlyCaptureGrabber::acquire(vpImage<vpRGBa> &I, FlyCapture2::TimeStamp &timestamp)
{
  this->open();

  FlyCapture2::Error error;
  // Retrieve an image
  error = m_camera.RetrieveBuffer(&m_rawImage);
  if (error != FlyCapture2::PGRERROR_OK) {
    error.PrintErrorTrace();
    std::cerr << "Cannot retrieve image from camera with serial " << getCameraSerial(m_index) << std::endl;
  }
  timestamp = m_rawImage.GetTimeStamp();

  // Create a converted image
  FlyCapture2::Image convertedImage;

  // Convert the raw image
  error = m_rawImage.Convert(FlyCapture2::PIXEL_FORMAT_RGBU, &convertedImage);
  if (error != FlyCapture2::PGRERROR_OK) {
    error.PrintErrorTrace();
    throw(vpException(vpException::fatalError, "Cannot convert image from camera with serial %u",
                      getCameraSerial(m_index)));
  }
  height = convertedImage.GetRows();
  width = convertedImage.GetCols();
  I.resize(height, width);

  unsigned char *data = convertedImage.GetData();
  unsigned int stride = convertedImage.GetStride();
  unsigned int Bps = convertedImage.GetBitsPerPixel() / 8; // Bytes per pixel
  // `I.bitmap` and `I[i]` are pointers to `vpRGBa` objects. While
  // `data` is a pointer to an array of 32-bit RGBU values with each
  // value a byte in the order of R, G, B and U and goes on.
  for (unsigned int i = 0; i < height; ++i) {
    for (unsigned int j = 0; j < width; ++j) {
      unsigned char *pp = data + i * stride + j * Bps;
      I[i][j].R = pp[0];
      I[i][j].G = pp[1];
      I[i][j].B = pp[2];
      I[i][j].A = pp[3];
    }
  }
}

void vpFlyCaptureGrabber::open(vpImage<unsigned char> &I)
{
  this->open();
  this->acquire(I);
}

void vpFlyCaptureGrabber::open(vpImage<vpRGBa> &I)
{
  this->open();
  this->acquire(I);
}

void vpFlyCaptureGrabber::open()
{
  this->connect();
  this->startCapture();
}

bool vpFlyCaptureGrabber::isCameraPowerAvailable()
{
  this->connect();

  const unsigned int powerReg = 0x400;
  unsigned int powerRegVal = 0;

  FlyCapture2::Error error;
  error = m_camera.ReadRegister(powerReg, &powerRegVal);
  if (error != FlyCapture2::PGRERROR_OK) {
    return false;
  }

  return ((powerRegVal & 0x00008000) != 0);
}

bool vpFlyCaptureGrabber::getCameraPower()
{
  if (!isCameraPowerAvailable())
    return false;
  const unsigned int powerReg = 0x610;
  unsigned int powerRegVal = 0;

  FlyCapture2::Error error;
  error = m_camera.ReadRegister(powerReg, &powerRegVal);
  if (error != FlyCapture2::PGRERROR_OK) {
    return false;
  }

  return ((powerRegVal & (0x1 << 31)) != 0);
}

void vpFlyCaptureGrabber::setCameraPower(bool on)
{
  this->connect();

  if (!isCameraPowerAvailable()) {
    throw(vpException(vpException::badValue, "Cannot power on camera. Feature not available"));
  }

  // Power on the camera
  const unsigned int powerReg = 0x610;
  unsigned int powerRegVal = 0;

  powerRegVal = (on == true) ? 0x80000000 : 0x0;

  FlyCapture2::Error error;
  error = m_camera.WriteRegister(powerReg, powerRegVal);
  if (error != FlyCapture2::PGRERROR_OK) {
    error.PrintErrorTrace();
    throw(vpException(vpException::fatalError, "Cannot power on the camera."));
  }

  unsigned int millisecondsToSleep = 100;
  unsigned int regVal = 0;
  unsigned int retries = 10;

  // Wait for camera to complete power-up
  do {
    vpTime::wait(millisecondsToSleep);
    error = m_camera.ReadRegister(powerReg, &regVal);
    if (error == FlyCapture2::PGRERROR_TIMEOUT) {
      // ignore timeout errors, camera may not be responding to
      // register reads during power-up
    } else if (error != FlyCapture2::PGRERROR_OK) {
      error.PrintErrorTrace();
      throw(vpException(vpException::fatalError, "Cannot power on the camera."));
    }

    retries--;
  } while ((regVal & powerRegVal) == 0 && retries > 0);

  // Check for timeout errors after retrying
  if (error == FlyCapture2::PGRERROR_TIMEOUT) {
    error.PrintErrorTrace();
    throw(vpException(vpException::fatalError, "Cannot power on the camera. Timeout occur"));
  }
}

vpFlyCaptureGrabber &vpFlyCaptureGrabber::operator>>(vpImage<unsigned char> &I)
{
  this->acquire(I);
  return *this;
}

vpFlyCaptureGrabber &vpFlyCaptureGrabber::operator>>(vpImage<vpRGBa> &I)
{
  this->acquire(I);
  return *this;
}

#else
// Work arround to avoid warning:
// libvisp_flycapture.a(vpFlyCaptureGrabber.cpp.o) has no symbols
void dummy_vpFlyCaptureGrabber(){};
#endif