vpFlyCaptureGrabber.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
 * ("GPL") version 2 as published by the Free Software Foundation.
 * 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<float>(std::min<float>(value, propInfo.absMax), propInfo.absMin);
      prop.absValue = value_;
      break;
    }
    case VALUE_A: {
      unsigned int value_ = std::max<unsigned int>(std::min<unsigned int>((unsigned int)value, propInfo.max), 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 width, unsigned int height)
{
  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(width, fmt7_info.maxWidth, fmt7_info.imageHStepSize);
  std::pair<unsigned int, unsigned int> roi_h = this->centerRoi(height, 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();
    throw (vpException(vpException::fatalError,
                       "Cannot retrieve image from camera with serial %u", getCameraSerial(m_index)));
  }
  timestamp = m_rawImage.GetTimeStamp();

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

  // 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)));
  }
  height = convertedImage.GetRows();
  width = convertedImage.GetCols();
  unsigned char *data = convertedImage.GetData();
  I.resize(height, width);
  memcpy(I.bitmap, data, height*width);
}

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();
    throw (vpException(vpException::fatalError,
                       "Cannot retrieve image from camera with serial %u", getCameraSerial(m_index)));
  }
  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();
  unsigned char *data = convertedImage.GetData();
  I.resize(height, width);
  unsigned int bps = convertedImage.GetBitsPerPixel();
  memcpy(I.bitmap, data, width*height*bps/8);
}


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.") );
  }

  const 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