CImageGrabber_FlyCapture2.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */
 
#include "hwdrivers-precomp.h"  // Precompiled headers
 
#include <mrpt/hwdrivers/CImageGrabber_FlyCapture2.h>
#include <mrpt/system/string_utils.h>
#include <mrpt/system/datetime.h>
 
#if MRPT_HAS_FLYCAPTURE2
#include <FlyCapture2.h>
using namespace FlyCapture2;
#endif
#if MRPT_HAS_TRICLOPS
#include <triclops.h>
#include <fc2triclops.h>
using namespace Fc2Triclops;
#endif
 
#if MRPT_HAS_OPENCV
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/imgproc/imgproc_c.h>
#endif
 
#define CHECK_FC2_ERROR(_err)                                    \
        {                                                            \
                if (_err != PGRERROR_OK)                                 \
                {                                                        \
                        THROW_EXCEPTION_FMT(                                 \
                                "FlyCapture2 error:\n%s", _err.GetDescription()) \
                }                                                        \
        }
#define CHECK_TRICLOPS_ERROR(_err)                                    \
        {                                                                 \
                if (_err != TriclopsErrorOk)                                  \
                {                                                             \
                        THROW_EXCEPTION_FMT(                                      \
                                "Triclops Error:\n'%s'", triclopsErrorToString(_err)) \
                }                                                             \
        }
#define FC2_CAM reinterpret_cast<FlyCapture2::Camera*>(m_camera)
#define FC2_CAM_INFO reinterpret_cast<FlyCapture2::CameraInfo*>(m_camera_info)
#define FC2_BUF_IMG reinterpret_cast<FlyCapture2::Image*>(m_img_buffer)
#define TRI_CONTEXT reinterpret_cast<TriclopsContext*>(m_triclops)
 
using namespace mrpt::hwdrivers;
using namespace std;
 
#if MRPT_HAS_FLYCAPTURE2
// Declare tables to convert strings to their #define values:
template <typename T>
struct fc2_str_val
{
        const char* str;
        T val;
};
 
const fc2_str_val<VideoMode> fc2_VideoMode_table[] = {
        {"VIDEOMODE_160x120YUV444", VIDEOMODE_160x120YUV444},
        {"VIDEOMODE_320x240YUV422", VIDEOMODE_320x240YUV422},
        {"VIDEOMODE_640x480YUV411", VIDEOMODE_640x480YUV411},
        {"VIDEOMODE_640x480YUV422", VIDEOMODE_640x480YUV422},
        {"VIDEOMODE_640x480RGB", VIDEOMODE_640x480RGB},
        {"VIDEOMODE_640x480Y8", VIDEOMODE_640x480Y8},
        {"VIDEOMODE_640x480Y16", VIDEOMODE_640x480Y16},
        {"VIDEOMODE_800x600YUV422", VIDEOMODE_800x600YUV422},
        {"VIDEOMODE_800x600RGB", VIDEOMODE_800x600RGB},
        {"VIDEOMODE_800x600Y8", VIDEOMODE_800x600Y8},
        {"VIDEOMODE_800x600Y16", VIDEOMODE_800x600Y16},
        {"VIDEOMODE_1024x768YUV422", VIDEOMODE_1024x768YUV422},
        {"VIDEOMODE_1024x768RGB", VIDEOMODE_1024x768RGB},
        {"VIDEOMODE_1024x768Y8", VIDEOMODE_1024x768Y8},
        {"VIDEOMODE_1024x768Y16", VIDEOMODE_1024x768Y16},
        {"VIDEOMODE_1280x960YUV422", VIDEOMODE_1280x960YUV422},
        {"VIDEOMODE_1280x960RGB", VIDEOMODE_1280x960RGB},
        {"VIDEOMODE_1280x960Y8", VIDEOMODE_1280x960Y8},
        {"VIDEOMODE_1280x960Y16", VIDEOMODE_1280x960Y16},
        {"VIDEOMODE_1600x1200YUV422", VIDEOMODE_1600x1200YUV422},
        {"VIDEOMODE_1600x1200RGB", VIDEOMODE_1600x1200RGB},
        {"VIDEOMODE_1600x1200Y8", VIDEOMODE_1600x1200Y8},
        {"VIDEOMODE_1600x1200Y16", VIDEOMODE_1600x1200Y16},
        {"VIDEOMODE_FORMAT7", VIDEOMODE_FORMAT7}};
fc2_str_val<FrameRate> fc2_FrameRate_table[] = {
        {"FRAMERATE_1_875", FlyCapture2::FRAMERATE_1_875},
        {"FRAMERATE_3_75", FlyCapture2::FRAMERATE_3_75},
        {"FRAMERATE_7_5", FlyCapture2::FRAMERATE_7_5},
        {"FRAMERATE_15", FlyCapture2::FRAMERATE_15},
        {"FRAMERATE_30", FlyCapture2::FRAMERATE_30},
        {"FRAMERATE_60", FlyCapture2::FRAMERATE_60},
        {"FRAMERATE_120", FlyCapture2::FRAMERATE_120},
        {"FRAMERATE_240", FlyCapture2::FRAMERATE_240},
        {"FRAMERATE_FORMAT7", FlyCapture2::FRAMERATE_FORMAT7}};
fc2_str_val<GrabMode> fc2_GrabMode_table[] = {{"DROP_FRAMES", DROP_FRAMES},
                                                                                          {"BUFFER_FRAMES", BUFFER_FRAMES}};
 
#define GET_CONV_TABLE(type)                                                   \
        vector<fc2_str_val<type>> fc2_vals_gen(type)                               \
        {                                                                          \
                size_t n = sizeof(fc2_##type##_table) / sizeof(fc2_##type##_table[0]); \
                vector<fc2_str_val<type>> vec(                                         \
                        &fc2_##type##_table[0], &fc2_##type##_table[n]);                   \
                return vec;                                                            \
        }
GET_CONV_TABLE(VideoMode)
GET_CONV_TABLE(FrameRate)
GET_CONV_TABLE(GrabMode)
 
template <typename T>
T fc2_defstr2num(const std::string& str)
{
        vector<fc2_str_val<T>> fc2_vals = fc2_vals_gen(T());
        const std::string s = mrpt::system::trim(str);
        for (size_t i = 0; i < fc2_vals.size(); i++)
        {
                if (mrpt::system::strCmpI(fc2_vals[i].str, s.c_str()))
                        return fc2_vals[i].val;
        }
        THROW_EXCEPTION_FMT("Error: Unknown FlyCapture2 constant: %s", s.c_str())
}
 
template <typename T>
const char* fc2_defnum2str(const T& val)
{
        vector<fc2_str_val<T>> fc2_vals = fc2_vals_gen(T());
        size_t i = static_cast<int>(val);
        if (i < fc2_vals.size())
                return fc2_vals[i].str;
        else
                THROW_EXCEPTION_FMT(
                        "Error: Unknown FlyCapture2 enum: %i", static_cast<int>(val))
}
#endif
 
//  Options: TCaptureOptions_bumblebee
// -------------------------------------------------------------
TCaptureOptions_FlyCapture2::TCaptureOptions_FlyCapture2()
        : camera_index(0),
          open_by_guid(false),
          videomode(),  //("VIDEOMODE_640x480Y8"),
          framerate(),  // ("FRAMERATE_30"),
          grabmode("BUFFER_FRAMES"),
          numBuffers(30),
          grabTimeout(-1),
          trigger_enabled(false),
          trigger_polarity(0),
          trigger_source(0),
          trigger_mode(0),
          strobe_enabled(false),
          strobe_source(0),
          strobe_polarity(0),
          strobe_delay(0.0f),
          strobe_duration(1.0f),
          autoexposure_auto(true),
          autoexposure_onOff(true),
          autoexposure_abs(true),
          autoexposure_EV(0.0f),
          shutter_auto(true),
          shutter_abs(true),
          shutter_time_ms(4.0f),
          gain_auto(true),
          gain_abs(true),
          gain_dB(0.0f),
          stereo_mode(false),
          get_rectified(false),
          rect_width(640),
          rect_height(480)
{
        memset(camera_guid, 0, 4 * sizeof(camera_guid[0]));
}
 
void TCaptureOptions_FlyCapture2::loadOptionsFrom(
        const mrpt::config::CConfigFileBase& cfg, const std::string& sect,
        const std::string& prefix)
{
        camera_index =
                cfg.read_int(sect, prefix + string("camera_index"), camera_index);
        open_by_guid =
                cfg.read_bool(sect, prefix + string("open_by_guid"), open_by_guid);
 
        if (open_by_guid)
        {
                string sGUID =
                        cfg.read_string(sect, prefix + string("camera_guid"), "", true);
                vector<string> sGUIDparts;
                mrpt::system::tokenize(sGUID, "- \t\r\n", sGUIDparts);
                ASSERTMSG_(
                        sGUIDparts.size() == 4,
                        "GUID format error: must have four blocks like XXX-XXX-XXX-XXX");
 
                for (int i = 0; i < 4; i++)
                        sscanf(sGUIDparts[i].c_str(), "%X", &camera_guid[i]);
        }
 
        videomode = cfg.read_string(sect, prefix + string("videomode"), videomode);
        framerate = cfg.read_string(sect, prefix + string("framerate"), framerate);
        grabmode = cfg.read_string(sect, prefix + string("grabmode"), grabmode);
        numBuffers =
                cfg.read_uint64_t(sect, prefix + string("numBuffers"), numBuffers);
        grabTimeout =
                cfg.read_int(sect, prefix + string("grabTimeout"), grabTimeout);
 
        trigger_enabled = cfg.read_bool(
                sect, prefix + string("trigger_enabled"), trigger_enabled);
        trigger_polarity = cfg.read_int(
                sect, prefix + string("trigger_polarity"), trigger_polarity);
        trigger_source =
                cfg.read_int(sect, prefix + string("trigger_source"), trigger_source);
        trigger_mode =
                cfg.read_int(sect, prefix + string("trigger_mode"), trigger_mode);
 
        strobe_enabled =
                cfg.read_bool(sect, prefix + string("strobe_enabled"), strobe_enabled);
        strobe_source =
                cfg.read_int(sect, prefix + string("strobe_source"), strobe_source);
        strobe_polarity =
                cfg.read_int(sect, prefix + string("strobe_polarity"), strobe_polarity);
        strobe_delay =
                cfg.read_float(sect, prefix + string("strobe_delay"), strobe_delay);
        strobe_duration = cfg.read_float(
                sect, prefix + string("strobe_duration"), strobe_duration);
 
        autoexposure_auto = cfg.read_bool(
                sect, prefix + string("autoexposure_auto"), autoexposure_auto);
        autoexposure_onOff = cfg.read_bool(
                sect, prefix + string("autoexposure_onOFf"), autoexposure_onOff);
        autoexposure_abs = cfg.read_bool(
                sect, prefix + string("autoexposure_abs"), autoexposure_abs);
        autoexposure_EV = cfg.read_float(
                sect, prefix + string("autoexposure_EV"), autoexposure_EV);
 
        shutter_auto =
                cfg.read_bool(sect, prefix + string("shutter_auto"), shutter_auto);
        shutter_abs =
                cfg.read_bool(sect, prefix + string("shutter_abs"), shutter_abs);
        shutter_time_ms = cfg.read_float(
                sect, prefix + string("shutter_time_ms"), shutter_time_ms);
 
        gain_auto = cfg.read_bool(sect, prefix + string("gain_auto"), gain_auto);
        gain_abs = cfg.read_bool(sect, prefix + string("gain_abs"), gain_abs);
        gain_dB = cfg.read_float(sect, prefix + string("gain_dB"), gain_dB);
 
        stereo_mode =
                cfg.read_bool(sect, prefix + string("stereo_mode"), stereo_mode);
        get_rectified =
                cfg.read_bool(sect, prefix + string("get_rectified"), get_rectified);
        rect_width =
                cfg.read_uint64_t(sect, prefix + string("rect_width"), rect_width);
        rect_height =
                cfg.read_uint64_t(sect, prefix + string("rect_height"), rect_height);
}
 
// ---------------------------------------------------------------
/** Default constructor */
CImageGrabber_FlyCapture2::CImageGrabber_FlyCapture2()
        : m_camera(nullptr),
          m_camera_info(nullptr),
          m_img_buffer(nullptr),
          m_triclops(nullptr)
{
#if MRPT_HAS_FLYCAPTURE2
        m_img_buffer = new FlyCapture2::Image();
#endif
}
 
/** Constructor + open */
CImageGrabber_FlyCapture2::CImageGrabber_FlyCapture2(
        const TCaptureOptions_FlyCapture2& options)
        : m_camera(nullptr),
          m_camera_info(nullptr),
          m_img_buffer(nullptr),
          m_triclops(nullptr)
{
#if MRPT_HAS_FLYCAPTURE2
        m_img_buffer = new FlyCapture2::Image();
#endif
        this->open(options);
}
 
/** Destructor */
CImageGrabber_FlyCapture2::~CImageGrabber_FlyCapture2()
{
#if MRPT_HAS_FLYCAPTURE2
        this->close();
        delete FC2_BUF_IMG;
        m_img_buffer = nullptr;
#endif
}
 
/** Tries to open the camera with the given options. Raises an exception on
 * error. \sa close() */
void CImageGrabber_FlyCapture2::open(
        const TCaptureOptions_FlyCapture2& options, const bool startCapture)
{
#if MRPT_HAS_FLYCAPTURE2
        FlyCapture2::Error fe;
 
        cout << "[CImageGrabber_FlyCapture2::open] FlyCapture2 version: "
                 << CImageGrabber_FlyCapture2::getFC2version() << std::endl;
 
        this->close();
        this->m_options = options;
 
        // Determine camera to open:
        // -----------------------------------
        PGRGuid guid;
        if (m_options.open_by_guid)
        {
                // Open by GUID:
                for (int i = 0; i < 4; i++) guid.value[i] = m_options.camera_guid[i];
        }
        else
        {
                // Open by camera index:
                BusManager busMgr;
                unsigned int numCameras;
                fe = busMgr.GetNumOfCameras(&numCameras);
                CHECK_FC2_ERROR(fe)
 
                if (m_options.camera_index >= numCameras)
                        THROW_EXCEPTION(
                                mrpt::format(
                                        "Error: camera_index to open is '%u', but only '%u' "
                                        "cameras were detected in the system.",
                                        m_options.camera_index, numCameras))
 
                fe = busMgr.GetCameraFromIndex(m_options.camera_index, &guid);
                CHECK_FC2_ERROR(fe)
        }
 
        // Connect to camera:
        m_camera = new FlyCapture2::Camera();
        m_camera_info = new FlyCapture2::CameraInfo();
 
        cout << mrpt::format(
                "[CImageGrabber_FlyCapture2::open] Opening camera with GUID= "
                "%08X-%08X-%08X-%08X...\n",
                guid.value[0], guid.value[1], guid.value[2], guid.value[3]);
        fe = FC2_CAM->Connect(&guid);
        CHECK_FC2_ERROR(fe)
        fe = FC2_CAM->GetCameraInfo(FC2_CAM_INFO);
        CHECK_FC2_ERROR(fe)
 
        const FlyCapture2::CameraInfo* ci = FC2_CAM_INFO;
 
        cout << mrpt::format(
                "[CImageGrabber_FlyCapture2::open] Camera connected ok:\n"
                " Serial number - %u\n"
                " Camera model - %s\n"
                " Camera vendor - %s\n"
                " Sensor - %s\n"
                " Resolution - %s\n"
                " Firmware version - %s\n"
                " Firmware build time - %s\n\n",
                ci->serialNumber, ci->modelName, ci->vendorName, ci->sensorInfo,
                ci->sensorResolution, ci->firmwareVersion, ci->firmwareBuildTime);
 
        // Set camera config:
        if (!m_options.videomode.empty() && !m_options.framerate.empty())
        {
                bool isSupported = false;
 
                if (!m_options.stereo_mode)
                {
                        FlyCapture2::VideoMode vidMode =
                                fc2_defstr2num<FlyCapture2::VideoMode>(m_options.videomode);
                        FlyCapture2::FrameRate vidRate =
                                fc2_defstr2num<FlyCapture2::FrameRate>(m_options.framerate);
 
                        fe = FC2_CAM->GetVideoModeAndFrameRateInfo(
                                vidMode, vidRate, &isSupported);
                        CHECK_FC2_ERROR(fe)
 
                        if (!isSupported)
                        {
                                FlyCapture2::VideoMode curVidMode;
                                FlyCapture2::FrameRate curVidRate;
                                fe =
                                        FC2_CAM->GetVideoModeAndFrameRate(&curVidMode, &curVidRate);
 
                                THROW_EXCEPTION(
                                        mrpt::format(
                                                "Camera mode '%s' + '%s' is not supported by this "
                                                "camera. Current mode is %d, current rate is %d.",
                                                m_options.videomode.c_str(),
                                                m_options.framerate.c_str(),
                                                static_cast<int>(curVidMode),
                                                static_cast<int>(curVidRate)))
                        }
 
                        fe = FC2_CAM->SetVideoModeAndFrameRate(vidMode, vidRate);
                        CHECK_FC2_ERROR(fe)
                }
                else
                {
#if MRPT_HAS_TRICLOPS
                        Fc2Triclops::ErrorType fte;
                        // Configure camera for Stereo mode
                        StereoCameraMode mode = TWO_CAMERA;
                        fte = setStereoMode(*(FC2_CAM), mode);
                        if (fte) handleFc2TriclopsError(fte, "setStereoMode");
 
                        // Generate Triclops context
 
                        m_triclops = new TriclopsContext;
                        fte = getContextFromCamera(FC2_CAM_INFO->serialNumber, TRI_CONTEXT);
                        if (fte != ERRORTYPE_OK)
                                handleFc2TriclopsError(fte, "getContextFromCamera");
 
                        //  ------------------------------------------------------
                        //   TRICLOPS CONFIGURATION
                        //  ------------------------------------------------------
                        // Current Format7 settings
                        /*
                        Format7ImageSettings f7settings;
                        unsigned int f7PacketSize;
                        float f7Percentage;
                        fe = FC2_CAM->GetFormat7Configuration(&f7settings, &f7PacketSize,
                        &f7Percentage);
                        CHECK_FC2_ERROR(fe)
                        */
 
                        TriclopsError te;
                        // Set rectified resolution
                        te = triclopsSetResolution(
                                *(TRI_CONTEXT), m_options.rect_height, m_options.rect_width);
                        CHECK_TRICLOPS_ERROR(te);
                        // Retrieve camera parameters
                        te = triclopsGetBaseline(*(TRI_CONTEXT), &m_baseline);
                        CHECK_TRICLOPS_ERROR(te);
                        te = triclopsGetFocalLength(*(TRI_CONTEXT), &m_focalLength);
                        CHECK_TRICLOPS_ERROR(te);
                        te = triclopsGetImageCenter(
                                *(TRI_CONTEXT), &m_centerRow, &m_centerCol);
                        CHECK_TRICLOPS_ERROR(te);
#else
                        THROW_EXCEPTION("MRPT compiled without support for Triclops");
#endif
                }
        }
 
        {
                FlyCapture2::VideoMode curVidMode;
                FlyCapture2::FrameRate curVidRate;
                fe = FC2_CAM->GetVideoModeAndFrameRate(&curVidMode, &curVidRate);
                if (fe == PGRERROR_OK)
                        cout << mrpt::format(
                                "[CImageGrabber_FlyCapture2::open] Current camera mode is %s, "
                                "current rate is %s.\n",
                                fc2_defnum2str<FlyCapture2::VideoMode>(curVidMode),
                                fc2_defnum2str<FlyCapture2::FrameRate>(curVidRate));
        }
 
        // Set trigger:
        FlyCapture2::TriggerModeInfo trigInfo;
        FC2_CAM->GetTriggerModeInfo(&trigInfo);
 
        FlyCapture2::TriggerMode trig;
        trig.onOff = m_options.trigger_enabled;
        if (m_options.trigger_enabled)
        {
                trig.mode = m_options.trigger_mode;
                trig.polarity = m_options.trigger_polarity;
                trig.source = m_options.trigger_source;
        }
        fe = FC2_CAM->SetTriggerMode(&trig);
        CHECK_FC2_ERROR(fe)
 
        // Strobe:
        if (m_options.strobe_enabled)
        {
                FlyCapture2::StrobeControl strobe;
 
                strobe.onOff = m_options.strobe_enabled;
                strobe.delay = m_options.strobe_delay;
                strobe.duration = m_options.strobe_duration;
                strobe.polarity = m_options.strobe_polarity;
                strobe.source = m_options.strobe_source;
 
                fe = FC2_CAM->SetStrobe(&strobe);
                CHECK_FC2_ERROR(fe)
        }
 
        // Set configs:
        FlyCapture2::FC2Config fc2conf;
        FC2_CAM->GetConfiguration(&fc2conf);
        CHECK_FC2_ERROR(fe)
 
        fc2conf.grabMode =
                fc2_defstr2num<FlyCapture2::GrabMode>(m_options.grabmode);
        if (m_options.grabTimeout >= 0) fc2conf.grabTimeout = m_options.grabTimeout;
 
        fc2conf.numBuffers = m_options.numBuffers;
 
        fe = FC2_CAM->SetConfiguration(&fc2conf);
        CHECK_FC2_ERROR(fe)
 
        // Autoexposure:
        {
                FlyCapture2::Property p;
                p.type = FlyCapture2::AUTO_EXPOSURE;
                FlyCapture2::Error error = FC2_CAM->GetProperty(&p);
                CHECK_FC2_ERROR(error)
                p.autoManualMode = m_options.autoexposure_auto;  // true=auto
                p.onOff = m_options.autoexposure_onOff;  // true=on
                p.absControl = m_options.autoexposure_abs;  // true=abs
                p.absValue =
                        m_options.autoexposure_EV;  // abs value in Exposure Value (EV)
                fe = FC2_CAM->SetProperty(&p);
                CHECK_FC2_ERROR(fe)
        }
 
        // Brightness:
        {
                FlyCapture2::Property p;
                p.type = FlyCapture2::BRIGHTNESS;
                FlyCapture2::Error error = FC2_CAM->GetProperty(&p);
                CHECK_FC2_ERROR(error)
                p.autoManualMode = true;  // true=auto
                // p.absControl = true;
                // p.absValue = Brightness;
                fe = FC2_CAM->SetProperty(&p);
                CHECK_FC2_ERROR(fe)
        }
 
        // Shutter:
        {
                FlyCapture2::Property p;
                p.type = FlyCapture2::SHUTTER;
                FlyCapture2::Error error = FC2_CAM->GetProperty(&p);
                CHECK_FC2_ERROR(error)
                p.autoManualMode = m_options.shutter_auto;  // true=auto
                p.absControl = m_options.shutter_abs;  // true=abs
                p.absValue = m_options.shutter_time_ms;
                // p.onOff = false;
                fe = FC2_CAM->SetProperty(&p);
                CHECK_FC2_ERROR(fe)
        }
 
        // Gain:
        {
                FlyCapture2::Property p;
                p.type = FlyCapture2::GAIN;
                FlyCapture2::Error error = FC2_CAM->GetProperty(&p);
                CHECK_FC2_ERROR(error)
                p.autoManualMode = m_options.gain_auto;  // true=auto
                p.absControl = m_options.gain_abs;  // true=abs
                p.absValue = m_options.gain_dB;  // abs value in dB (decibeles)
                // p.onOff = false;
                fe = FC2_CAM->SetProperty(&p);
                CHECK_FC2_ERROR(fe)
        }
 
        // Framecounter:
        EmbeddedImageInfo eii;
        fe = FC2_CAM->GetEmbeddedImageInfo(&eii);
        if (fe == PGRERROR_OK)
        {
                if (eii.frameCounter.available) eii.frameCounter.onOff = true;
                if (eii.timestamp.available) eii.timestamp.onOff = true;
                if (eii.exposure.available) eii.exposure.onOff = true;
                if (eii.brightness.available) eii.brightness.onOff = true;
 
                // Enable all:
                FC2_CAM->SetEmbeddedImageInfo(&eii);
        }
 
        // Start:
        if (startCapture) this->startCapture();
#else
        THROW_EXCEPTION("MRPT compiled without support for FlyCapture2");
#endif
}
 
/** Start the actual image capture of the camera. Must be called after open(),
 * only when "startCapture" was set to false. */
void CImageGrabber_FlyCapture2::startCapture()
{
#if MRPT_HAS_FLYCAPTURE2
        if (!m_camera)
        {
                THROW_EXCEPTION("Camera is not opened. Call open() first.");
        }
 
        FlyCapture2::Error error = FC2_CAM->StartCapture();
        CHECK_FC2_ERROR(error)
 
#else
        THROW_EXCEPTION("MRPT compiled without support for FlyCapture2");
#endif
}
 
/** Starts a synchronous capture of several cameras, which must have been
 * already opened. */
void CImageGrabber_FlyCapture2::startSyncCapture(
        int numCameras, const CImageGrabber_FlyCapture2** cameras_array)
{
#if MRPT_HAS_FLYCAPTURE2
 
        std::vector<const FlyCapture2::Camera*> cam_ptrs(numCameras);
 
        for (int i = 0; i < numCameras; i++)
        {
                const CImageGrabber_FlyCapture2* obj = cameras_array[i];
                if (!obj->m_camera)
                {
                        THROW_EXCEPTION_FMT(
                                "Camera #%i in list is not opened. Call open() first.", i)
                }
 
                FlyCapture2::Camera* cam =
                        reinterpret_cast<FlyCapture2::Camera*>(obj->m_camera);
                cam_ptrs[i] = cam;
        }
 
        if (!cam_ptrs.empty())
        {
                FlyCapture2::Error error = FlyCapture2::Camera::StartSyncCapture(
                        cam_ptrs.size(), &cam_ptrs[0]);
                CHECK_FC2_ERROR(error)
        }
#else
        THROW_EXCEPTION("MRPT compiled without support for FlyCapture2");
#endif
}
 
/** Stop capture. */
void CImageGrabber_FlyCapture2::stopCapture()
{
#if MRPT_HAS_FLYCAPTURE2
        if (m_camera)
        {
                Error error;
 
                // Stop grabbing:
                error = FC2_CAM->StopCapture();
                CHECK_FC2_ERROR(error)
        }
#else
        THROW_EXCEPTION("MRPT compiled without support for FlyCapture2");
#endif
}
 
/** Closes the opened camera, if any. Called automatically on object
 * destruction. */
void CImageGrabber_FlyCapture2::close()
{
#if MRPT_HAS_FLYCAPTURE2
        try
        {
                this->stopCapture();
        }
        catch (...)
        {
        }
 
        // Disconnect the camera
        try
        {
                if (m_camera) FC2_CAM->Disconnect();
        }
        catch (...)
        {
        }
 
        // Delete objects:
        try
        {
                if (m_camera) delete FC2_CAM;
        }
        catch (...)
        {
        }
        try
        {
                if (m_camera_info) delete FC2_CAM_INFO;
        }
        catch (...)
        {
        }
#if MRPT_HAS_TRICLOPS
        try
        {
                if (m_triclops) delete TRI_CONTEXT;
        }
        catch (...)
        {
        }
#endif
 
        m_camera = nullptr;
        m_camera_info = nullptr;
 
#else
        THROW_EXCEPTION("MRPT compiled without support for FlyCapture2");
#endif
}
 
/** Returns the PGR FlyCapture2 library version */
std::string CImageGrabber_FlyCapture2::getFC2version()
{
#if MRPT_HAS_FLYCAPTURE2
        FlyCapture2::FC2Version fc2Version;
        FlyCapture2::Utilities::GetLibraryVersion(&fc2Version);
        return mrpt::format(
                "%d.%d.%d.%d", fc2Version.major, fc2Version.minor, fc2Version.type,
                fc2Version.build);
#else
        THROW_EXCEPTION("MRPT compiled without support for FlyCapture2");
#endif
}
 
/*-------------------------------------------------------------
                                        get the image - MONO
 -------------------------------------------------------------*/
// Grab image from the camera. This method blocks until the next frame is
// captured.
// return: false on any error.
bool CImageGrabber_FlyCapture2::getObservation(
        mrpt::obs::CObservationImage& out_observation)
{
#if MRPT_HAS_FLYCAPTURE2
        if (!m_camera)
        {
                std::cerr << "[CImageGrabber_FlyCapture2::getObservation] Camera is "
                                         "not opened. Call open() first.\n";
                return false;
        }
        try
        {
                FlyCapture2::Error error;
                FlyCapture2::Image image;
                error = FC2_CAM->RetrieveBuffer(&image);
                CHECK_FC2_ERROR(error)
                FlyCapture2::TimeStamp timestamp = image.GetTimeStamp();
                // White balance, etc.
                // FlyCapture2::ImageMetadata imd = image.GetMetadata();
                // Determine if it's B/W or color:
                FlyCapture2::PixelFormat pf = image.GetPixelFormat();
                const bool is_color =
                        pf == PIXEL_FORMAT_RGB8 || pf == PIXEL_FORMAT_RGB16 ||
                        pf == PIXEL_FORMAT_S_RGB16 || pf == PIXEL_FORMAT_RAW8 ||
                        pf == PIXEL_FORMAT_RAW16 || pf == PIXEL_FORMAT_RAW12 ||
                        pf == PIXEL_FORMAT_BGR || pf == PIXEL_FORMAT_BGRU ||
                        pf == PIXEL_FORMAT_RGBU || pf == PIXEL_FORMAT_BGR16 ||
                        pf == PIXEL_FORMAT_BGRU16 || pf == PIXEL_FORMAT_422YUV8_JPEG;
                // Decode image:
                error = image.Convert(
                        is_color ? PIXEL_FORMAT_BGR : PIXEL_FORMAT_MONO8, FC2_BUF_IMG);
                CHECK_FC2_ERROR(error)
                // Convert PGR FlyCapture2 image ==> OpenCV format:
                unsigned int img_rows, img_cols, img_stride;
                FC2_BUF_IMG->GetDimensions(&img_rows, &img_cols, &img_stride);
                out_observation.image.loadFromMemoryBuffer(
                        img_cols, img_rows, is_color, FC2_BUF_IMG->GetData());
                // It seems timestamp is not always correctly filled in the incoming
                // imgs:
                if (timestamp.seconds != 0)
                        out_observation.timestamp = mrpt::system::time_tToTimestamp(
                                timestamp.seconds + 1e-6 * timestamp.microSeconds);
                else
                        out_observation.timestamp = mrpt::system::now();
                return true;
        }
        catch (std::exception& e)
        {
                std::cerr << "[CImageGrabber_FlyCapture2::getObservation] Error:\n"
                                  << e.what() << std::endl;
                return false;
        }
#else
        THROW_EXCEPTION("MRPT compiled without support for FlyCapture2");
#endif
}
 
/*-------------------------------------------------------------
                                        get the image - STEREO
 -------------------------------------------------------------*/
// Grab image from the camera. This method blocks until the next frame is
// captured.
// return: false on any error.
bool CImageGrabber_FlyCapture2::getObservation(
        mrpt::obs::CObservationStereoImages& out_observation)
{
#if MRPT_HAS_FLYCAPTURE2 && MRPT_HAS_TRICLOPS && MRPT_HAS_OPENCV
        if (!m_camera)
        {
                std::cerr << "[CImageGrabber_FlyCapture2::getObservation] Camera is "
                                         "not opened. Call open() first.\n";
                return false;
        }
 
        try
        {
                FlyCapture2::Error ferr;
                Fc2Triclops::ErrorType fterr;
                TriclopsError te;
                FlyCapture2::Image image;
                ferr = FC2_CAM->RetrieveBuffer(&image);
                CHECK_FC2_ERROR(ferr)
                mrpt::system::TTimeStamp ts_retrieved = mrpt::system::now();
                FlyCapture2::TimeStamp timestamp = image.GetTimeStamp();
 
                // White balance, etc.
                // FlyCapture2::ImageMetadata imd = image.GetMetadata();
 
                // ------------------------------------------
                // Extract images from common interleaved image:
                // ------------------------------------------
                IplImage* imageIpl[2];  // Output pair of images
                FlyCapture2::Image rawImage[2];
 
                // Convert the pixel interleaved raw data to de-interleaved raw data
                fterr = Fc2Triclops::unpackUnprocessedRawOrMono16Image(
                        image, true,
                        rawImage[0],  // Right image
                        rawImage[1]);  // Left image
                if (fterr != Fc2Triclops::ERRORTYPE_OK)
                {
                        Fc2Triclops::handleFc2TriclopsError(
                                fterr, "unprocessedRawOrMono16Image()");
                        return false;
                }
 
                // Convert each raw image to RGBU image (for color images)
                unsigned int img_rows, img_cols, img_stride;
                for (int i = 0; i < 2; ++i)
                {
                        FlyCapture2::Image rgbuImage;
                        ferr = rawImage[i].SetColorProcessing(FlyCapture2::HQ_LINEAR);
                        CHECK_FC2_ERROR(ferr)
                        ferr = rawImage[i].Convert(PIXEL_FORMAT_BGRU, &rgbuImage);
                        CHECK_FC2_ERROR(ferr)
 
                        unsigned char* data::Ptr;  // To store Ipl converted image pointer
                        if (m_options.get_rectified)  // If rectified
                        {
                                // Use the rgbu single image to build up a packed (rbgu)
                                // TriclopsInput.
                                // A packed triclops input will contain a single image with 32
                                // bpp.
                                TriclopsInput triclopsColorInput;
                                te = triclopsBuildPackedTriclopsInput(
                                        rgbuImage.GetCols(), rgbuImage.GetRows(),
                                        rgbuImage.GetStride(),
                                        (unsigned long)image.GetTimeStamp().seconds,
                                        (unsigned long)image.GetTimeStamp().microSeconds,
                                        rgbuImage.GetData(), &triclopsColorInput);
 
                                // Do rectification
                                TriclopsPackedColorImage rectPackColImg;
                                te = triclopsRectifyPackedColorImage(
                                        *(TRI_CONTEXT), i == 0 ? TriCam_RIGHT : TriCam_LEFT,
                                        const_cast<TriclopsInput*>(&triclopsColorInput),
                                        &rectPackColImg);
                                CHECK_TRICLOPS_ERROR(te)
 
                                // Set image properties for reallocation
                                img_rows = rectPackColImg.nrows;
                                img_cols = rectPackColImg.ncols;
                                img_stride = rectPackColImg.rowinc;
                                data::Ptr = (unsigned char*)rectPackColImg.data;
                        }
                        else  // If not rectified
                        {
                                rgbuImage.GetDimensions(&img_rows, &img_cols, &img_stride);
                                data::Ptr = rgbuImage.GetData();
                        }
                        // Convert PGR image ==> OpenCV format:
                        IplImage* tmpImage =
                                cvCreateImage(cvSize(img_cols, img_rows), IPL_DEPTH_8U, 4);
 
                        // Copy image data
                        memcpy(tmpImage->imageData, data::Ptr, img_rows * img_stride);
                        tmpImage->widthStep = img_stride;
                        // Convert images to BGR (3 channels) and set origins
                        imageIpl[i] =
                                cvCreateImage(cvSize(img_cols, img_rows), IPL_DEPTH_8U, 3);
                        cvCvtColor(tmpImage, imageIpl[i], CV_BGRA2BGR);
                        imageIpl[i]->origin = tmpImage->origin;
                        // Release temp images
                        cvReleaseImage(&tmpImage);
                }
 
                /*-------------------------------------------------------------
                                                        Fill output stereo observation
                 -------------------------------------------------------------*/
                out_observation.imageRight.setFromIplImage(imageIpl[0]);  // Right cam.
                out_observation.imageLeft.setFromIplImage(imageIpl[1]);  // Left cam.
 
                // It seems timestamp is not always correctly filled in the incoming
                // imgs:
                if (timestamp.seconds != 0)
                        out_observation.timestamp = mrpt::system::time_tToTimestamp(
                                timestamp.seconds + 1e-6 * timestamp.microSeconds);
                else
                        out_observation.timestamp = ts_retrieved;
 
                out_observation.rightCameraPose.x(m_baseline);
                out_observation.rightCameraPose.y(0);
                out_observation.rightCameraPose.z(0);
 
                out_observation.rightCameraPose.quat().r(1);
                out_observation.rightCameraPose.quat().x(0);
                out_observation.rightCameraPose.quat().y(0);
                out_observation.rightCameraPose.quat().z(0);
 
                out_observation.cameraPose.x(0);
                out_observation.cameraPose.y(0);
                out_observation.cameraPose.z(0);
 
                out_observation.cameraPose.quat().r(1);
                out_observation.cameraPose.quat().x(0);
                out_observation.cameraPose.quat().y(0);
                out_observation.cameraPose.quat().z(0);
 
                out_observation.leftCamera.setIntrinsicParamsFromValues(
                        m_focalLength, m_focalLength, m_centerCol, m_centerRow);
                out_observation.rightCamera.setIntrinsicParamsFromValues(
                        m_focalLength, m_focalLength, m_centerCol, m_centerRow);
                return true;
        }
        catch (std::exception& e)
        {
                std::cerr << "[CImageGrabber_FlyCapture2::getObservation] Error:\n"
                                  << e.what() << std::endl;
                return false;
        }
#else
        THROW_EXCEPTION(
                "MRPT compiled without support for FlyCapture2, Triclops or OpenCV")
#endif
}