CImageGrabber_FlyCapture2.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "hwdrivers-precomp.h"  // Precompiled headers

#include <mrpt/hwdrivers/CImageGrabber_FlyCapture2.h>
#include <mrpt/system/datetime.h>
#include <mrpt/system/string_utils.h>

#if MRPT_HAS_FLYCAPTURE2
#include <FlyCapture2.h>
using namespace FlyCapture2;
#endif
#if MRPT_HAS_TRICLOPS
#include <fc2triclops.h>
#include <triclops.h>
using namespace Fc2Triclops;
#endif

#if MRPT_HAS_OPENCV
#include <opencv2/imgproc/imgproc_c.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#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()
    : videomode(),  //("VIDEOMODE_640x480Y8"),
      framerate(),  // ("FRAMERATE_30"),
      grabmode("BUFFER_FRAMES")

{
    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);
    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()
{
#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 (const 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;
        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:
        // ------------------------------------------
        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_BGR, &rgbuImage);
            CHECK_FC2_ERROR(ferr)

            // get unrectified images:
            rgbuImage.GetDimensions(&img_rows, &img_cols, &img_stride);
            uint8_t* ptrData = rgbuImage.GetData();

            // Convert PGR image ==> OpenCV format:
            auto tmpImg =
                cv::Mat(img_rows, img_cols, CV_8UC3, ptrData, img_stride);

            // Fill output stereo observation
            auto& im =
                (i == 0 ? out_observation.imageRight
                        : out_observation.imageLeft);
            im = mrpt::img::CImage(tmpImg, mrpt::img::DEEP_COPY);
        }

        out_observation.hasImageRight = true;

        // 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 (const 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
}