CCameraSensor.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/config/CConfigFile.h>
#include <mrpt/config/CConfigFileMemory.h>
#include <mrpt/config/CConfigFilePrefixer.h>
#include <mrpt/gui/WxSubsystem.h>
#include <mrpt/gui/WxUtils.h>
#include <mrpt/hwdrivers/CCameraSensor.h>
#include <mrpt/obs/CObservationImage.h>
#include <mrpt/obs/CObservationStereoImages.h>
#include <mrpt/obs/CRawlog.h>
#include <mrpt/obs/CSensoryFrame.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/system/os.h>
#include <mrpt/system/string_utils.h>
#include <mrpt/system/thread_name.h>

#include <memory>

using namespace mrpt;
using namespace mrpt::hwdrivers;
using namespace mrpt::gui;
using namespace mrpt::obs;
using namespace mrpt::config;
using namespace mrpt::system;
using namespace mrpt::io;
using namespace mrpt::serialization;
using namespace mrpt::img;
using namespace std;
using namespace std::literals;

IMPLEMENTS_GENERIC_SENSOR(CCameraSensor, mrpt::hwdrivers)

/* -----------------------------------------------------
                Constructor
   ----------------------------------------------------- */
CCameraSensor::CCameraSensor()
    : mrpt::system::COutputLogger("CCameraSensor"),
      m_sensorPose(),
      m_grabber_type("opencv"),

      m_cv_camera_type("CAMERA_CV_AUTODETECT"),
      m_cv_options(),

      m_dc1394_options(),

      m_svs_options(),

      // ---
      m_img_dir_url(""),
      m_img_dir_left_format("imL_%04d.jpg"),
      m_img_dir_right_format("imR_%04d.jpg"),

      m_external_image_saver_count(std::thread::hardware_concurrency()),

      m_hook_pre_save(nullptr)

{
    m_sensorLabel = "CAMERA";
    m_state = CGenericSensor::ssInitializing;
}

/* -----------------------------------------------------
                initialize
   ----------------------------------------------------- */
void CCameraSensor::initialize()
{
    cout << "[CCameraSensor::initialize] Opening camera..." << endl;
    close();

    // Select type of device
    m_grabber_type = trim(lowerCase(m_grabber_type));
    m_cv_camera_type = trim(upperCase(m_cv_camera_type));

    if (m_grabber_type == "opencv")
    {
        // OpenCV driver:
        TCameraType ct;
        try
        {
            ct = mrpt::typemeta::TEnumType<TCameraType>::name2value(
                m_cv_camera_type);
        }
        catch (std::exception&)
        {
            m_state = CGenericSensor::ssError;
            throw;
        }
        cout << format(
            "[CCameraSensor::initialize] opencv camera, index: %i type: "
            "%i...\n",
            int(m_cv_camera_index), (int)ct);
        m_cap_cv = std::make_unique<CImageGrabber_OpenCV>(
            m_cv_camera_index, ct, m_cv_options);

        if (!m_cap_cv->isOpen())
        {
            m_state = CGenericSensor::ssError;
            THROW_EXCEPTION(
                "[CCameraSensor::initialize] ERROR: Couldn't open OpenCV "
                "camera.");
        }
    }
    else if (m_grabber_type == "dc1394")
    {
        // m_cap_dc1394
        cout << format(
            "[CCameraSensor::initialize] dc1394 camera, GUID: 0x%lX  "
            "UNIT:%d...\n",
            long(m_dc1394_camera_guid), m_dc1394_camera_unit);
        m_cap_dc1394 = std::make_unique<CImageGrabber_dc1394>(
            m_dc1394_camera_guid, m_dc1394_camera_unit, m_dc1394_options,
            true /* verbose */);

        if (!m_cap_dc1394->isOpen())
        {
            m_state = CGenericSensor::ssError;
            THROW_EXCEPTION(
                "[CCameraSensor::initialize] ERROR: Couldn't open dc1394 "
                "camera.");
        }
    }
    else if (m_grabber_type == "bumblebee_dc1394")
    {
        cout << format(
            "[CCameraSensor::initialize] bumblebee_libdc1394 camera: "
            "GUID:0x%08X Index:%i FPS:%f...\n",
            (unsigned int)(m_bumblebee_dc1394_camera_guid),
            m_bumblebee_dc1394_camera_unit, m_bumblebee_dc1394_framerate);
        m_cap_bumblebee_dc1394 =
            std::make_unique<CStereoGrabber_Bumblebee_libdc1394>(
                m_bumblebee_dc1394_camera_guid, m_bumblebee_dc1394_camera_unit,
                m_bumblebee_dc1394_framerate);
    }
    else if (m_grabber_type == "svs")
    {
        cout << format(
            "[CCameraSensor::initialize] SVS camera: %u...\n",
            (unsigned int)(m_svs_camera_index));
        m_cap_svs = std::make_unique<CStereoGrabber_SVS>(
            m_svs_camera_index, m_svs_options);
    }
    else if (m_grabber_type == "ffmpeg")
    {
        // m_cap_ffmpeg
        cout << format(
            "[CCameraSensor::initialize] FFmpeg stream: %s...\n",
            m_ffmpeg_url.c_str());
        m_cap_ffmpeg = std::make_unique<CFFMPEG_InputStream>();

        if (!m_cap_ffmpeg->openURL(m_ffmpeg_url, m_capture_grayscale))
        {
            m_state = CGenericSensor::ssError;
            THROW_EXCEPTION_FMT(
                "Error opening FFmpeg stream: %s", m_ffmpeg_url.c_str());
        }
    }
    else if (m_grabber_type == "swissranger")
    {
        cout << "[CCameraSensor::initialize] SwissRanger camera...\n";
        m_cap_swissranger = std::make_unique<CSwissRanger3DCamera>();

        m_cap_swissranger->setOpenFromUSB(m_sr_open_from_usb);
        m_cap_swissranger->setOpenIPAddress(m_sr_ip_address);

        m_cap_swissranger->setSave3D(m_sr_save_3d);
        m_cap_swissranger->setSaveRangeImage(m_sr_save_range_img);
        m_cap_swissranger->setSaveIntensityImage(m_sr_save_intensity_img);
        m_cap_swissranger->setSaveConfidenceImage(m_sr_save_confidence);

        if (!m_path_for_external_images.empty())
            m_cap_swissranger->setPathForExternalImages(
                m_path_for_external_images);

        // Open it:
        try
        {
            m_cap_swissranger
                ->initialize();  // This will launch an exception if needed.
        }
        catch (std::exception&)
        {
            m_state = CGenericSensor::ssError;
            throw;
        }
    }
    else if (m_grabber_type == "kinect")
    {
        cout << "[CCameraSensor::initialize] Kinect camera...\n";
        m_cap_kinect = std::make_unique<CKinect>();
        m_cap_kinect->enableGrab3DPoints(m_kinect_save_3d);
        m_cap_kinect->enableGrabDepth(m_kinect_save_range_img);
        m_cap_kinect->enableGrabRGB(m_kinect_save_intensity_img);
        m_cap_kinect->setVideoChannel(
            m_kinect_video_rgb ? CKinect::VIDEO_CHANNEL_RGB
                               : CKinect::VIDEO_CHANNEL_IR);

        if (!m_path_for_external_images.empty())
            m_cap_kinect->setPathForExternalImages(m_path_for_external_images);

        // Open it:
        try
        {
            m_cap_kinect
                ->initialize();  // This will launch an exception if needed.
        }
        catch (std::exception&)
        {
            m_state = CGenericSensor::ssError;
            throw;
        }
    }
    else if (m_grabber_type == "openni2")
    {
        cout << "[CCameraSensor::initialize] OpenNI2 sensor...\n";
        m_cap_openni2 = std::make_unique<COpenNI2Sensor>();
        m_cap_openni2->enableGrab3DPoints(m_kinect_save_3d);  // It uses the
        // same options as
        // the Kinect
        // grabber
        m_cap_openni2->enableGrabDepth(m_kinect_save_range_img);
        m_cap_openni2->enableGrabRGB(m_kinect_save_intensity_img);

        if (!m_path_for_external_images.empty())
            m_cap_openni2->setPathForExternalImages(m_path_for_external_images);

        // Open it:
        try
        {
            m_cap_openni2
                ->initialize();  // This will launch an exception if needed.
        }
        catch (const std::exception& e)
        {
            m_state = CGenericSensor::ssError;
            throw e;
        }
    }
    else if (m_grabber_type == "image_dir")
    {
        // m_cap_image_dir
        cout << format(
            "[CCameraSensor::initialize] Image dir: %s...\n",
            m_img_dir_url.c_str());
        m_cap_image_dir = std::make_unique<std::string>();
    }
    else if (m_grabber_type == "rawlog")
    {
        // m_cap_rawlog
        cout << format(
            "[CCameraSensor::initialize] Rawlog stream: %s...\n",
            m_rawlog_file.c_str());
        m_cap_rawlog = std::make_unique<CFileGZInputStream>();

        if (!m_cap_rawlog->open(m_rawlog_file))
        {
            m_state = CGenericSensor::ssError;
            THROW_EXCEPTION_FMT(
                "Error opening rawlog file: %s", m_rawlog_file.c_str());
        }
        // File open OK.
        // Localize the external images directory of this rawlog, if it exists:
        m_rawlog_detected_images_dir =
            CRawlog::detectImagesDirectory(m_rawlog_file);
    }
    else if (m_grabber_type == "flycap")
    {
        cout << "[CCameraSensor::initialize] PGR FlyCapture2 camera...\n";
        try
        {
            // Open camera and start capture:
            m_cap_flycap =
                std::make_unique<CImageGrabber_FlyCapture2>(m_flycap_options);
        }
        catch (std::exception&)
        {
            m_state = CGenericSensor::ssError;
            throw;
        }
    }
    else if (m_grabber_type == "flycap_stereo")
    {
        cout
            << "[CCameraSensor::initialize] PGR FlyCapture2 stereo camera...\n";
        try
        {
            // Open camera and start capture:
            m_cap_flycap_stereo_l =
                std::make_unique<CImageGrabber_FlyCapture2>();
            m_cap_flycap_stereo_r =
                std::make_unique<CImageGrabber_FlyCapture2>();

            cout << "[CCameraSensor::initialize] PGR FlyCapture2 stereo "
                    "camera: Opening LEFT camera...\n";
            m_cap_flycap_stereo_l->open(
                m_flycap_stereo_options[0], false /* don't start grabbing */);

            cout << "[CCameraSensor::initialize] PGR FlyCapture2 stereo "
                    "camera: Opening RIGHT camera...\n";
            m_cap_flycap_stereo_r->open(
                m_flycap_stereo_options[1], false /* don't start grabbing */);

            // Now, start grabbing "simultaneously":
            if (m_fcs_start_synch_capture)
            {
                const CImageGrabber_FlyCapture2* cams[2] = {
                    m_cap_flycap_stereo_l.get(), m_cap_flycap_stereo_r.get()};
                CImageGrabber_FlyCapture2::startSyncCapture(2, cams);
            }
            else
            {
                m_cap_flycap_stereo_l->startCapture();
                m_cap_flycap_stereo_r->startCapture();
            }
        }
        catch (std::exception&)
        {
            m_state = CGenericSensor::ssError;
            throw;
        }
    }
    else if (m_grabber_type == "duo3d")
    {
        // m_cap_duo3D
        cout << "[CCameraSensor::initialize] DUO3D stereo camera ...\n";

        // Open it:
        try
        {
            m_cap_duo3d = std::make_unique<CDUO3DCamera>(m_duo3d_options);
        }
        catch (const std::exception& e)
        {
            m_state = CGenericSensor::ssError;
            throw e;
        }
    }
    else if (m_grabber_type == "myntd")
    {
        cout << "[CCameraSensor::initialize] MYNTEYE-D camera ...\n";

        // Open it:
        try
        {
            m_myntd = std::make_unique<CMyntEyeCamera>(m_myntd_options);
        }
        catch (const std::exception& e)
        {
            m_state = CGenericSensor::ssError;
            throw e;
        }
    }
    else
        THROW_EXCEPTION_FMT(
            "Unknown 'grabber_type' found: %s", m_grabber_type.c_str());

    // Change state:
    cout << "[CCameraSensor::initialize] Done!" << endl;
    m_state = CGenericSensor::ssWorking;

    // Launch independent thread?
    if (m_external_images_own_thread)
    {
        m_threadImagesSaverShouldEnd = false;

        m_threadImagesSaver.clear();
        m_threadImagesSaver.resize(m_external_image_saver_count);

        m_toSaveList.clear();
        m_toSaveList.resize(m_external_image_saver_count);

        for (unsigned int i = 0; i < m_external_image_saver_count; ++i)
        {
            m_threadImagesSaver[i] =
                std::thread(&CCameraSensor::thread_save_images, this, i);

            mrpt::system::thread_name(
                "CameraSaveImages", m_threadImagesSaver[i]);
        }
    }
}

/* -----------------------------------------------------
                close
   ----------------------------------------------------- */
void CCameraSensor::close()
{
    m_cap_cv.reset();
    m_cap_dc1394.reset();
    m_cap_flycap.reset();
    m_cap_flycap_stereo_l.reset();
    m_cap_flycap_stereo_r.reset();
    m_cap_bumblebee_dc1394.reset();
    m_cap_ffmpeg.reset();
    m_cap_rawlog.reset();
    m_cap_swissranger.reset();
    m_cap_kinect.reset();
    m_cap_svs.reset();
    m_cap_image_dir.reset();
    m_cap_duo3d.reset();

    m_state = CGenericSensor::ssInitializing;

    // Wait for threads:
    if (!m_threadImagesSaver.empty())
    {
        m_threadImagesSaverShouldEnd = true;
        for (auto& i : m_threadImagesSaver) i.join();
    }
}

/* -----------------------------------------------------
                loadConfig_sensorSpecific
   ----------------------------------------------------- */
void CCameraSensor::loadConfig_sensorSpecific(
    const mrpt::config::CConfigFileBase& configSource,
    const std::string& iniSection)
{
    // At this point, my parent class CGenericSensor has already loaded its
    // params:
    //  Since cameras are special, we'll take control over "m_grab_decimation"
    //  so
    //  external image files are not saved just to be discarded later on...
    if (m_grab_decimation > 0)
    {
        m_camera_grab_decimator = m_grab_decimation;
        m_camera_grab_decimator_counter = 0;
        // Reset in parent:
        m_grab_decimation = 0;
    }
    else
        m_camera_grab_decimator = m_camera_grab_decimator_counter = 0;

    m_grabber_type = configSource.read_string_first_word(
        iniSection, "grabber_type", m_grabber_type);
    MRPT_LOAD_HERE_CONFIG_VAR(
        preview_decimation, int, m_preview_decimation, configSource, iniSection)
    MRPT_LOAD_HERE_CONFIG_VAR(
        preview_reduction, int, m_preview_reduction, configSource, iniSection)

    // OpenCV options:
    m_cv_camera_type = configSource.read_string_first_word(
        iniSection, "cv_camera_type", m_cv_camera_type);
    m_cv_camera_index =
        configSource.read_int(iniSection, "cv_camera_index", m_cv_camera_index);

    m_cv_options.frame_width = configSource.read_int(
        iniSection, "cv_frame_width", m_cv_options.frame_width);
    m_cv_options.frame_height = configSource.read_int(
        iniSection, "cv_frame_height", m_cv_options.frame_height);
    m_cv_options.gain =
        configSource.read_double(iniSection, "cv_gain", m_cv_options.gain);
    m_cv_options.ieee1394_fps = configSource.read_double(
        iniSection, "cv_fps", m_cv_options.ieee1394_fps);

    m_capture_grayscale =
        configSource.read_bool(iniSection, "capture_grayscale", false);

    m_cv_options.ieee1394_grayscale = m_capture_grayscale;

    // dc1394 options:
    MRPT_LOAD_HERE_CONFIG_VAR(
        dc1394_camera_guid, uint64_t, m_dc1394_camera_guid, configSource,
        iniSection)
    MRPT_LOAD_HERE_CONFIG_VAR(
        dc1394_camera_unit, int, m_dc1394_camera_unit, configSource, iniSection)

    MRPT_LOAD_HERE_CONFIG_VAR(
        dc1394_frame_width, int, m_dc1394_options.frame_width, configSource,
        iniSection)
    MRPT_LOAD_HERE_CONFIG_VAR(
        dc1394_frame_height, int, m_dc1394_options.frame_height, configSource,
        iniSection)

    MRPT_LOAD_HERE_CONFIG_VAR(
        dc1394_mode7, int, m_dc1394_options.mode7, configSource, iniSection)

    MRPT_LOAD_HERE_CONFIG_VAR(
        dc1394_shutter, int, m_dc1394_options.shutter, configSource, iniSection)
    MRPT_LOAD_HERE_CONFIG_VAR(
        dc1394_gain, int, m_dc1394_options.gain, configSource, iniSection)
    MRPT_LOAD_HERE_CONFIG_VAR(
        dc1394_gamma, int, m_dc1394_options.gamma, configSource, iniSection)
    MRPT_LOAD_HERE_CONFIG_VAR(
        dc1394_brightness, int, m_dc1394_options.brightness, configSource,
        iniSection)
    MRPT_LOAD_HERE_CONFIG_VAR(
        dc1394_exposure, int, m_dc1394_options.exposure, configSource,
        iniSection)
    MRPT_LOAD_HERE_CONFIG_VAR(
        dc1394_sharpness, int, m_dc1394_options.sharpness, configSource,
        iniSection)
    MRPT_LOAD_HERE_CONFIG_VAR(
        dc1394_white_balance, int, m_dc1394_options.white_balance, configSource,
        iniSection)

    MRPT_LOAD_HERE_CONFIG_VAR(
        dc1394_shutter_mode, int, m_dc1394_options.shutter_mode, configSource,
        iniSection)
    MRPT_LOAD_HERE_CONFIG_VAR(
        dc1394_gain_mode, int, m_dc1394_options.gain_mode, configSource,
        iniSection)
    MRPT_LOAD_HERE_CONFIG_VAR(
        dc1394_gamma_mode, int, m_dc1394_options.gamma_mode, configSource,
        iniSection)
    MRPT_LOAD_HERE_CONFIG_VAR(
        dc1394_brightness_mode, int, m_dc1394_options.brightness_mode,
        configSource, iniSection)
    MRPT_LOAD_HERE_CONFIG_VAR(
        dc1394_exposure_mode, int, m_dc1394_options.exposure_mode, configSource,
        iniSection)
    MRPT_LOAD_HERE_CONFIG_VAR(
        dc1394_sharpness_mode, int, m_dc1394_options.sharpness_mode,
        configSource, iniSection)
    MRPT_LOAD_HERE_CONFIG_VAR(
        dc1394_white_balance_mode, int, m_dc1394_options.white_balance_mode,
        configSource, iniSection)

    MRPT_LOAD_HERE_CONFIG_VAR(
        dc1394_trigger_power, int, m_dc1394_options.trigger_power, configSource,
        iniSection)
    MRPT_LOAD_HERE_CONFIG_VAR(
        dc1394_trigger_mode, int, m_dc1394_options.trigger_mode, configSource,
        iniSection)
    MRPT_LOAD_HERE_CONFIG_VAR(
        dc1394_trigger_source, int, m_dc1394_options.trigger_source,
        configSource, iniSection)
    MRPT_LOAD_HERE_CONFIG_VAR(
        dc1394_trigger_polarity, int, m_dc1394_options.trigger_polarity,
        configSource, iniSection)
    MRPT_LOAD_HERE_CONFIG_VAR(
        dc1394_ring_buffer_size, int, m_dc1394_options.ring_buffer_size,
        configSource, iniSection)

    // Bumblebee_dc1394 options:
    MRPT_LOAD_HERE_CONFIG_VAR(
        bumblebee_dc1394_camera_guid, uint64_t, m_bumblebee_dc1394_camera_guid,
        configSource, iniSection)
    MRPT_LOAD_HERE_CONFIG_VAR(
        bumblebee_dc1394_camera_unit, int, m_bumblebee_dc1394_camera_unit,
        configSource, iniSection)
    MRPT_LOAD_HERE_CONFIG_VAR(
        bumblebee_dc1394_framerate, double, m_bumblebee_dc1394_framerate,
        configSource, iniSection)

    // SVS options:
    m_svs_camera_index = configSource.read_int(
        iniSection, "svs_camera_index", m_svs_camera_index);
    m_svs_options.frame_width = configSource.read_int(
        iniSection, "svs_frame_width", m_svs_options.frame_width);
    m_svs_options.frame_height = configSource.read_int(
        iniSection, "svs_frame_height", m_svs_options.frame_height);
    m_svs_options.framerate = configSource.read_double(
        iniSection, "svs_framerate", m_svs_options.framerate);
    m_svs_options.m_NDisp =
        configSource.read_int(iniSection, "svs_NDisp", m_svs_options.m_NDisp);
    m_svs_options.m_Corrsize = configSource.read_int(
        iniSection, "svs_Corrsize", m_svs_options.m_Corrsize);
    m_svs_options.m_LR =
        configSource.read_int(iniSection, "svs_LR", m_svs_options.m_LR);
    m_svs_options.m_Thresh =
        configSource.read_int(iniSection, "svs_Thresh", m_svs_options.m_Thresh);
    m_svs_options.m_Unique =
        configSource.read_int(iniSection, "svs_Unique", m_svs_options.m_Unique);
    m_svs_options.m_Horopter = configSource.read_int(
        iniSection, "svs_Horopter", m_svs_options.m_Horopter);
    m_svs_options.m_SpeckleSize = configSource.read_int(
        iniSection, "svs_SpeckleSize", m_svs_options.m_SpeckleSize);
    m_svs_options.m_procesOnChip = configSource.read_bool(
        iniSection, "svs_procesOnChip", m_svs_options.m_procesOnChip);
    m_svs_options.m_calDisparity = configSource.read_bool(
        iniSection, "svs_calDisparity", m_svs_options.m_calDisparity);

    // FFmpeg options:
    m_ffmpeg_url = mrpt::system::trim(
        configSource.read_string(iniSection, "ffmpeg_url", m_ffmpeg_url));

    // Rawlog options:
    m_rawlog_file = mrpt::system::trim(
        configSource.read_string(iniSection, "rawlog_file", m_rawlog_file));
    m_rawlog_camera_sensor_label = mrpt::system::trim(configSource.read_string(
        iniSection, "rawlog_camera_sensor_label",
        m_rawlog_camera_sensor_label));

    // Image directory options:
    m_img_dir_url = mrpt::system::trim(
        configSource.read_string(iniSection, "image_dir_url", m_img_dir_url));
    m_img_dir_left_format = mrpt::system::trim(configSource.read_string(
        iniSection, "left_format", m_img_dir_left_format));
    m_img_dir_right_format = mrpt::system::trim(
        configSource.read_string(iniSection, "right_format", ""));
    m_img_dir_start_index =
        configSource.read_int(iniSection, "start_index", m_img_dir_start_index);
    ;
    m_img_dir_end_index =
        configSource.read_int(iniSection, "end_index", m_img_dir_end_index);

    m_img_dir_is_stereo = !m_img_dir_right_format.empty();
    m_img_dir_counter = m_img_dir_start_index;

    // DUO3D Camera options:
    m_duo3d_options.loadOptionsFrom(configSource, "DUO3DOptions");

    // SwissRanger options:
    m_sr_open_from_usb =
        configSource.read_bool(iniSection, "sr_use_usb", m_sr_open_from_usb);
    m_sr_ip_address =
        configSource.read_string(iniSection, "sr_IP", m_sr_ip_address);

    m_sr_save_3d =
        configSource.read_bool(iniSection, "sr_grab_3d", m_sr_save_3d);
    m_sr_save_intensity_img = configSource.read_bool(
        iniSection, "sr_grab_grayscale", m_sr_save_intensity_img);
    m_sr_save_range_img = configSource.read_bool(
        iniSection, "sr_grab_range", m_sr_save_range_img);
    m_sr_save_confidence = configSource.read_bool(
        iniSection, "sr_grab_confidence", m_sr_save_confidence);

    m_kinect_save_3d =
        configSource.read_bool(iniSection, "kinect_grab_3d", m_kinect_save_3d);
    m_kinect_save_intensity_img = configSource.read_bool(
        iniSection, "kinect_grab_intensity", m_kinect_save_intensity_img);
    m_kinect_save_range_img = configSource.read_bool(
        iniSection, "kinect_grab_range", m_kinect_save_range_img);
    m_kinect_video_rgb = configSource.read_bool(
        iniSection, "kinect_video_rgb", m_kinect_video_rgb);

    // FlyCap:
    m_flycap_options.loadOptionsFrom(configSource, iniSection, "flycap_");

    // Myntd:
    {
        mrpt::config::CConfigFilePrefixer c(configSource, "", "myntd_");
        m_myntd_options.loadFromConfigFile(c, iniSection);
    }

    // FlyCap stereo
    m_fcs_start_synch_capture = configSource.read_bool(
        iniSection, "fcs_start_synch_capture", m_fcs_start_synch_capture);
    m_flycap_stereo_options[0].loadOptionsFrom(
        configSource, iniSection, "fcs_LEFT_");
    m_flycap_stereo_options[1].loadOptionsFrom(
        configSource, iniSection, "fcs_RIGHT_");

    // Special stuff: FPS
    map<double, grabber_dc1394_framerate_t> map_fps;
    map<double, grabber_dc1394_framerate_t>::iterator it_fps;
    map_fps[1.875] = FRAMERATE_1_875;
    map_fps[3.75] = FRAMERATE_3_75;
    map_fps[7.5] = FRAMERATE_7_5;
    map_fps[15] = FRAMERATE_15;
    map_fps[30] = FRAMERATE_30;
    map_fps[60] = FRAMERATE_60;
    map_fps[120] = FRAMERATE_120;
    map_fps[240] = FRAMERATE_240;

    // ... for dc1394
    double the_fps =
        configSource.read_double(iniSection, "dc1394_framerate", 15.0);
    it_fps = map_fps.find(the_fps);
    if (it_fps == map_fps.end())
        THROW_EXCEPTION_FMT(
            "ERROR: DC1394 framerate seems to be not a valid number: %f",
            the_fps);

    m_dc1394_options.framerate = it_fps->second;

    // Special stuff: color encoding:
    map<string, grabber_dc1394_color_coding_t> map_color;
    map<string, grabber_dc1394_color_coding_t>::iterator it_color;
#define ADD_COLOR_MAP(c) map_color[#c] = c;
    ADD_COLOR_MAP(COLOR_CODING_MONO8)
    ADD_COLOR_MAP(COLOR_CODING_YUV411)
    ADD_COLOR_MAP(COLOR_CODING_YUV422)
    ADD_COLOR_MAP(COLOR_CODING_YUV444)
    ADD_COLOR_MAP(COLOR_CODING_RGB8)
    ADD_COLOR_MAP(COLOR_CODING_MONO16)

    string the_color_coding =
        mrpt::system::upperCase(configSource.read_string_first_word(
            iniSection, "dc1394_color_coding", "COLOR_CODING_YUV422"));
    it_color = map_color.find(the_color_coding);
    if (it_color == map_color.end())
        THROW_EXCEPTION_FMT(
            "ERROR: Color coding seems not to be valid : '%s'",
            the_color_coding.c_str());
    m_dc1394_options.color_coding = it_color->second;

    m_external_images_format = mrpt::system::trim(configSource.read_string(
        iniSection, "external_images_format", m_external_images_format));
    m_external_images_jpeg_quality = configSource.read_int(
        iniSection, "external_images_jpeg_quality",
        m_external_images_jpeg_quality);
    m_external_images_own_thread = configSource.read_bool(
        iniSection, "external_images_own_thread", m_external_images_own_thread);
    m_external_image_saver_count = configSource.read_int(
        iniSection, "external_images_own_thread_count",
        m_external_image_saver_count);

    // Sensor pose:
    m_sensorPose.setFromValues(
        configSource.read_float(iniSection, "pose_x", 0),
        configSource.read_float(iniSection, "pose_y", 0),
        configSource.read_float(iniSection, "pose_z", 0),
        DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0)),
        DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0)),
        DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0)));
}

/* -----------------------------------------------------
                Destructor
   ----------------------------------------------------- */
CCameraSensor::~CCameraSensor()
{
    close();

    m_preview_win1.reset();
    m_preview_win2.reset();
}
/* -----------------------------------------------------
                getNextFrame
----------------------------------------------------- */
CObservation::Ptr CCameraSensor::getNextFrame()
{
    vector<CSerializable::Ptr> out_obs;
    getNextFrame(out_obs);
    return std::dynamic_pointer_cast<CObservation>(out_obs[0]);
}

/* -----------------------------------------------------
                getNextFrame
----------------------------------------------------- */
void CCameraSensor::getNextFrame(vector<CSerializable::Ptr>& out_obs)
{
    CObservationImage::Ptr obs;
    CObservationStereoImages::Ptr stObs;
    CObservation3DRangeScan::Ptr
        obs3D;  // 3D range image, also with an intensity channel
    CObservationIMU::Ptr obsIMU;  // IMU observation grabbed by DUO3D cameras

    bool capture_ok = false;

    if (m_cap_cv)
    {
        obs = std::make_shared<CObservationImage>();
        if (!m_cap_cv->getObservation(*obs))
        {  // Error
            m_state = CGenericSensor::ssError;
            THROW_EXCEPTION("Error grabbing image");
        }
        else
            capture_ok = true;
    }
    else if (m_cap_dc1394)
    {
        obs = std::make_shared<CObservationImage>();
        if (!m_cap_dc1394->getObservation(*obs))
        {  // Error
            m_state = CGenericSensor::ssError;
            THROW_EXCEPTION("Error grabbing image");
        }
        else
            capture_ok = true;
    }
    else if (m_cap_swissranger)
    {
        obs3D = std::make_shared<CObservation3DRangeScan>();

        bool there_is_obs, hardware_error;
        m_cap_swissranger->getNextObservation(
            *obs3D, there_is_obs, hardware_error);

        if (!there_is_obs || hardware_error)
        {  // Error
            m_state = CGenericSensor::ssError;
            THROW_EXCEPTION("Error grabbing image from SwissRanger camera.");
        }
        else
            capture_ok = true;
    }
    else if (m_cap_kinect)
    {
        obs3D = std::make_shared<CObservation3DRangeScan>();

        // Specially at start-up, there may be a delay grabbing so a few calls
        // return false: add a timeout.
        const mrpt::system::TTimeStamp t0 = mrpt::system::now();
        double max_timeout = 3.0;  // seconds

        // If we have an "MRPT_CCAMERA_KINECT_TIMEOUT_MS" environment variable,
        // use that timeout instead:
        const char* envVal = getenv("MRPT_CCAMERA_KINECT_TIMEOUT_MS");
        if (envVal) max_timeout = atoi(envVal) * 0.001;

        bool there_is_obs, hardware_error;
        do
        {
            m_cap_kinect->getNextObservation(
                *obs3D, there_is_obs, hardware_error);
            if (!there_is_obs) std::this_thread::sleep_for(1ms);
        } while (!there_is_obs && mrpt::system::timeDifference(
                                      t0, mrpt::system::now()) < max_timeout);

        if (!there_is_obs || hardware_error)
        {  // Error
            m_state = CGenericSensor::ssError;
            THROW_EXCEPTION("Error grabbing image from Kinect camera.");
        }
        else
            capture_ok = true;
    }
    else if (m_cap_openni2)
    {
        obs3D = std::make_shared<CObservation3DRangeScan>();
        // Specially at start-up, there may be a delay grabbing so a few calls
        // return false: add a timeout.
        const mrpt::system::TTimeStamp t0 = mrpt::system::now();
        double max_timeout = 3.0;  // seconds
        bool there_is_obs, hardware_error;
        do
        {
            m_cap_openni2->getNextObservation(
                *obs3D, there_is_obs, hardware_error);
            if (!there_is_obs) std::this_thread::sleep_for(1ms);
        } while (!there_is_obs && mrpt::system::timeDifference(
                                      t0, mrpt::system::now()) < max_timeout);

        if (!there_is_obs || hardware_error)
        {  // Error
            m_state = CGenericSensor::ssError;
            THROW_EXCEPTION("Error grabbing image from OpenNI2 camera.");
        }
        else
            capture_ok = true;
    }
    else if (m_cap_bumblebee_dc1394)
    {
        stObs = std::make_shared<CObservationStereoImages>();
        if (!m_cap_bumblebee_dc1394->getStereoObservation(*stObs))
        {
            m_state = CGenericSensor::ssError;
            THROW_EXCEPTION("Error grabbing stereo images");
        }
        else
        {
            capture_ok = true;
        }
    }
    else if (m_cap_svs)
    {
        stObs = std::make_shared<CObservationStereoImages>();

        if (!m_cap_svs->getStereoObservation(*stObs))
        {
            // Error
            m_state = CGenericSensor::ssError;
            THROW_EXCEPTION("Error grabbing disparity images");
        }
        else
            capture_ok = true;
    }
    else if (m_cap_ffmpeg)
    {
        static bool anyGood = false;
        mrpt::img::CImage im;
        if (!m_cap_ffmpeg->retrieveFrame(im))
        {  // Error
            m_state = CGenericSensor::ssError;
            if (!anyGood)
            {
                THROW_EXCEPTION("Error grabbing image");
            }
            else
            {
                MRPT_LOG_THROTTLE_WARN(
                    2.0,
                    "ffmpeg capture driver: Failed to get frame (temporary "
                    "error or EOF?)");
                capture_ok = true;
            }
        }
        else
        {
            obs = std::make_shared<CObservationImage>();
            obs->image = std::move(im);

            anyGood = true;
            capture_ok = true;
        }
    }
    else if (m_cap_image_dir)
    {
        if (m_img_dir_counter > m_img_dir_end_index)
        {
            m_state = CGenericSensor::ssError;
            THROW_EXCEPTION("Reached end index.");
        }

        std::string auxL = format(
            "%s/%s", m_img_dir_url.c_str(), m_img_dir_left_format.c_str());
        if (m_img_dir_is_stereo)
        {
            stObs = std::make_shared<CObservationStereoImages>();
            if (!stObs->imageLeft.loadFromFile(
                    format(auxL.c_str(), m_img_dir_counter)))
            {
                m_state = CGenericSensor::ssError;
                THROW_EXCEPTION("Error reading images from directory");
            }
            std::string auxR = format(
                "%s/%s", m_img_dir_url.c_str(), m_img_dir_right_format.c_str());
            if (!stObs->imageRight.loadFromFile(
                    format(auxR.c_str(), m_img_dir_counter++)))
            {
                m_state = CGenericSensor::ssError;
                THROW_EXCEPTION("Error reading images from directory");
            }
            else
                capture_ok = true;
        }
        else
        {
            // use only left image prefix
            obs = std::make_shared<CObservationImage>();
            if (!obs->image.loadFromFile(
                    format(auxL.c_str(), m_img_dir_counter++)))
            {
                m_state = CGenericSensor::ssError;
                THROW_EXCEPTION("Error reading images from directory");
            }
            else
                capture_ok = true;
        }
    }
    else if (m_cap_rawlog)
    {
        // Read in a loop until we found at least one image:
        //  Assign to: obs && stObs
        CSerializable::Ptr newObs;
        while (!obs && !stObs && !obs3D)
        {
            archiveFrom(*m_cap_rawlog) >> newObs;
            if (IS_DERIVED(*newObs, CObservation))
            {
                CObservation::Ptr o =
                    std::dynamic_pointer_cast<CObservation>(newObs);
                if (!m_rawlog_camera_sensor_label.empty() &&
                    m_rawlog_camera_sensor_label != o->sensorLabel)
                    continue;

                if (IS_CLASS(*o, CObservationImage))
                    obs = std::dynamic_pointer_cast<CObservationImage>(o);
                else if (IS_CLASS(*o, CObservationStereoImages))
                    stObs =
                        std::dynamic_pointer_cast<CObservationStereoImages>(o);
                else if (IS_CLASS(*o, CObservation3DRangeScan))
                    obs3D =
                        std::dynamic_pointer_cast<CObservation3DRangeScan>(o);
            }
            else if (IS_CLASS(*newObs, CSensoryFrame))
            {
                CSensoryFrame::Ptr sf =
                    std::dynamic_pointer_cast<CSensoryFrame>(newObs);

                for (auto& o : *sf)
                {
                    if (!m_rawlog_camera_sensor_label.empty() &&
                        m_rawlog_camera_sensor_label != o->sensorLabel)
                        continue;

                    if (IS_CLASS(*o, CObservationImage))
                    {
                        obs = std::dynamic_pointer_cast<CObservationImage>(o);
                        break;
                    }
                    else if (IS_CLASS(*o, CObservationStereoImages))
                    {
                        stObs =
                            std::dynamic_pointer_cast<CObservationStereoImages>(
                                o);
                        break;
                    }
                    else if (IS_CLASS(*o, CObservation3DRangeScan))
                    {
                        obs3D =
                            std::dynamic_pointer_cast<CObservation3DRangeScan>(
                                o);
                        break;
                    }
                }
            }
            if (obs || stObs || obs3D)
            {
                // We must convert externally stored images into "normal
                // in-memory" images.
                const std::string old_dir =
                    CImage::getImagesPathBase();  // Save current
                CImage::setImagesPathBase(m_rawlog_detected_images_dir);

                if (obs && obs->image.isExternallyStored())
                    obs->image.loadFromFile(
                        obs->image.getExternalStorageFileAbsolutePath());

                if (obs3D && obs3D->hasIntensityImage &&
                    obs3D->intensityImage.isExternallyStored())
                    obs3D->intensityImage.loadFromFile(
                        obs3D->intensityImage
                            .getExternalStorageFileAbsolutePath());

                if (stObs && stObs->imageLeft.isExternallyStored())
                    stObs->imageLeft.loadFromFile(
                        stObs->imageLeft.getExternalStorageFileAbsolutePath());

                if (stObs && stObs->hasImageRight &&
                    stObs->imageRight.isExternallyStored())
                    stObs->imageRight.loadFromFile(
                        stObs->imageRight.getExternalStorageFileAbsolutePath());

                if (stObs && stObs->hasImageDisparity &&
                    stObs->imageDisparity.isExternallyStored())
                    stObs->imageDisparity.loadFromFile(
                        stObs->imageDisparity
                            .getExternalStorageFileAbsolutePath());

                CImage::setImagesPathBase(old_dir);  // Restore
            }
            else
                continue;  // Keep reading
        }
        capture_ok = true;
    }
    else if (m_cap_flycap)
    {
        bool ok;
        if (!m_cap_flycap->isStereo())  // Mono image
        {
            obs = std::make_shared<CObservationImage>();
            ok = m_cap_flycap->getObservation(*obs);
        }
        else  // Stereo camera connected
        {
            stObs = std::make_shared<CObservationStereoImages>();
            ok = m_cap_flycap->getObservation(*stObs);
        }

        if (!ok)
        {  // Error
            m_state = CGenericSensor::ssError;
            THROW_EXCEPTION("Error grabbing image");
        }
        else
            capture_ok = true;
    }
    else if (m_cap_flycap_stereo_l && m_cap_flycap_stereo_r)
    {
        stObs = std::make_shared<CObservationStereoImages>();

        CObservationImage obsL, obsR;

        bool ok1, ok2 = false;

        ok1 = m_cap_flycap_stereo_r->getObservation(obsL);
        if (ok1) ok2 = m_cap_flycap_stereo_l->getObservation(obsR);

        if (!ok1 || !ok2)
        {
            // Error
            m_state = CGenericSensor::ssError;
            THROW_EXCEPTION("Error grabbing disparity images");
        }
        else
        {
            // Joint the two images as one stereo:
            const double At =
                mrpt::system::timeDifference(obsL.timestamp, obsR.timestamp);
            if (std::abs(At) > 0.1)
            {
                cout << "[CCamera, flycap_stereo] Warning: Too large delay "
                        "between left & right images: "
                     << At << " sec.\n";
            }

            // It seems that the timestamp is not always filled in from FlyCap
            // driver?
            stObs->timestamp = (obsL.timestamp != INVALID_TIMESTAMP)
                                   ? obsL.timestamp
                                   : mrpt::system::now();
            stObs->imageLeft = std::move(obsL.image);
            stObs->imageRight = std::move(obsR.image);
            capture_ok = true;
        }
    }
    else if (m_cap_duo3d)
    {
        stObs = std::make_shared<CObservationStereoImages>();
        obsIMU = std::make_shared<CObservationIMU>();

        bool thereIsIMG, thereIsIMU;
        m_cap_duo3d->getObservations(*stObs, *obsIMU, thereIsIMG, thereIsIMU);
        if (!thereIsIMG)
        {
            m_state = CGenericSensor::ssError;
            THROW_EXCEPTION("Error getting observations from DUO3D camera.");
        }
        else if (m_cap_duo3d->captureIMUIsSet() && !thereIsIMU)
        {
            cout << "[CCamera, duo3d] Warning: There are no IMU data from the "
                    "device. Only images are being grabbed.";
        }
        capture_ok = true;
    }
    else if (m_myntd)
    {
        obs3D = std::make_shared<CObservation3DRangeScan>();

        bool thereIsObs = m_myntd->getObservation(*obs3D);
        static int noObsCnt = 0;

        if (!thereIsObs)
        {
            // obs3D.reset();
            if (noObsCnt++ > 100)
            {
                m_state = CGenericSensor::ssError;
                THROW_EXCEPTION(
                    "Error getting observations from MYNTEYE-D camera.");
            }
        }
        else
        {
            noObsCnt = 0;
        }
        capture_ok = true;
    }
    else
    {
        THROW_EXCEPTION(
            "There is no initialized camera driver: has 'initialize()' been "
            "called?");
    }

    ASSERT_(capture_ok);

    // Are we supposed to do a decimation??
    m_camera_grab_decimator_counter++;
    if (m_camera_grab_decimator_counter < m_camera_grab_decimator)
    {
        // Done here:
        out_obs.push_back(CObservation::Ptr());
        return;
    }
    // Continue as normal:
    m_camera_grab_decimator_counter = 0;

    // Allow temporary failures to get any frame
    // ASSERT_(obs || stObs || obs3D || obsIMU);

    // If we grabbed an image: prepare it and add it to the internal queue:
    if (obs)
    {
        obs->sensorLabel = m_sensorLabel;
        obs->setSensorPose(m_sensorPose);
    }
    else if (stObs)
    {
        stObs->sensorLabel = (m_cap_duo3d && m_cap_duo3d->captureIMUIsSet())
                                 ? m_sensorLabel + "_IMG"
                                 : m_sensorLabel;
        stObs->setSensorPose(m_sensorPose);
    }
    else if (obs3D)
    {
        obs3D->sensorLabel = m_sensorLabel;
        obs3D->setSensorPose(m_sensorPose);
    }
    if (obsIMU)
    {
        obsIMU->sensorLabel = m_sensorLabel + "_IMU";
        obsIMU->setSensorPose(m_sensorPose);
    }

    // Convert to grayscale if the user wants so and  the driver did ignored us:
    if (m_capture_grayscale)
    {
        if (obs)
        {
            if (obs->image.isColor()) obs->image = obs->image.grayscale();
        }
        else if (stObs)
        {
            if (stObs->imageLeft.isColor())
                stObs->imageLeft = stObs->imageLeft.grayscale();
            if (stObs->hasImageRight && stObs->imageRight.isColor())
                stObs->imageRight = stObs->imageRight.grayscale();
            if (stObs->hasImageDisparity && stObs->imageDisparity.isColor())
                stObs->imageDisparity = stObs->imageDisparity.grayscale();
        }
        else if (obs3D)
        {
            if (obs3D->hasIntensityImage && obs3D->intensityImage.isColor())
                obs3D->intensityImage = obs3D->intensityImage.grayscale();
        }
    }

    // Before saving to disk, keep a copy for display, if needed:
    CImage img4gui, img4guiR;
    if (m_preview_win1 && m_preview_win1->isOpen())
    {
        if (stObs)
        {
            img4gui = stObs->imageLeft.makeDeepCopy();
            img4guiR = stObs->imageRight.makeDeepCopy();
        }
        else if (obs)
            img4gui = obs->image.makeDeepCopy();
        else
            img4gui = obs3D->intensityImage.makeDeepCopy();
    }

    // External storage?
    // If true, we'll return nothing, but the observation will be
    // inserted from the thread.
    bool delayed_insertion_in_obs_queue = false;
    if (!m_path_for_external_images.empty())
    {
        if (stObs)  // If we have grabbed an stereo observation ...
        {  // Stereo obs  -------
            if (m_external_images_own_thread)
            {
                m_csToSaveList.lock();

                // Select the "m_toSaveList" with the shortest pending queue:
                size_t idx_min = 0;
                for (size_t i = 0; i < m_toSaveList.size(); ++i)
                    if (m_toSaveList[i].size() < m_toSaveList[idx_min].size())
                        idx_min = i;
                // Insert:
                m_toSaveList[idx_min].insert(
                    TListObsPair(stObs->timestamp, stObs));

                m_csToSaveList.unlock();

                delayed_insertion_in_obs_queue = true;
            }
            else
            {
                const string filNameL =
                    fileNameStripInvalidChars(trim(m_sensorLabel)) +
                    format(
                        "_L_%f.%s", (double)timestampTotime_t(stObs->timestamp),
                        m_external_images_format.c_str());
                const string filNameR =
                    fileNameStripInvalidChars(trim(m_sensorLabel)) +
                    format(
                        "_R_%f.%s", (double)timestampTotime_t(stObs->timestamp),
                        m_external_images_format.c_str());
                const string filNameD =
                    fileNameStripInvalidChars(trim(m_sensorLabel)) +
                    format(
                        "_D_%f.%s", (double)timestampTotime_t(stObs->timestamp),
                        m_external_images_format.c_str());
                // cout << "[CCameraSensor] Saving " << filName << endl;
                stObs->imageLeft.saveToFile(
                    m_path_for_external_images + string("/") + filNameL,
                    m_external_images_jpeg_quality);
                stObs->imageLeft.setExternalStorage(filNameL);

                if (stObs->hasImageRight)
                {
                    stObs->imageRight.saveToFile(
                        m_path_for_external_images + string("/") + filNameR,
                        m_external_images_jpeg_quality);
                    stObs->imageRight.setExternalStorage(filNameR);
                }
                if (stObs->hasImageDisparity)
                {
                    stObs->imageDisparity.saveToFile(
                        m_path_for_external_images + string("/") + filNameD,
                        m_external_images_jpeg_quality);
                    stObs->imageDisparity.setExternalStorage(filNameD);
                }
            }
        }
        else if (obs)
        {  // Monocular image obs  -------
            if (m_external_images_own_thread)
            {
                m_csToSaveList.lock();

                // Select the "m_toSaveList" with the shortest pending queue:
                size_t idx_min = 0;
                for (size_t i = 0; i < m_toSaveList.size(); ++i)
                    if (m_toSaveList[i].size() < m_toSaveList[idx_min].size())
                        idx_min = i;

                // Insert:
                m_toSaveList[idx_min].insert(TListObsPair(obs->timestamp, obs));

                m_csToSaveList.unlock();
                delayed_insertion_in_obs_queue = true;
            }
            else
            {
                string filName =
                    fileNameStripInvalidChars(trim(m_sensorLabel)) +
                    format(
                        "_%f.%s", (double)timestampTotime_t(obs->timestamp),
                        m_external_images_format.c_str());
                // cout << "[CCameraSensor] Saving " << filName << endl;
                obs->image.saveToFile(
                    m_path_for_external_images + string("/") + filName,
                    m_external_images_jpeg_quality);
                obs->image.setExternalStorage(filName);
            }
        }  // end else
    }

    // Show preview??
    if (m_preview_decimation > 0)
    {  // Yes
        if (++m_preview_counter > m_preview_decimation)
        {
            m_preview_counter = 0;

            // Create window the first time:
            if (!m_preview_win1)
            {
                string caption = string("Preview of ") + m_sensorLabel;
                if (stObs) caption += "-LEFT";
                if (m_preview_decimation > 1)
                    caption +=
                        format(" (decimation: %i)", m_preview_decimation);
                m_preview_win1 = mrpt::gui::CDisplayWindow::Create(caption);
            }
            if (stObs && !m_preview_win2)
            {
                string caption = string("Preview of ") + m_sensorLabel;
                if (stObs) caption += "-RIGHT";
                if (m_preview_decimation > 1)
                    caption +=
                        format(" (decimation: %i)", m_preview_decimation);
                m_preview_win2 = mrpt::gui::CDisplayWindow::Create(caption);
            }
            // Monocular image or Left from a stereo pair:
            if (m_preview_win1->isOpen() && img4gui.getWidth() > 0)
            {
                // Apply image reduction?
                if (m_preview_reduction >= 2)
                {
                    unsigned int w = img4gui.getWidth();
                    unsigned int h = img4gui.getHeight();
                    CImage auxImg;
                    img4gui.scaleImage(
                        auxImg, w / m_preview_reduction,
                        h / m_preview_reduction, IMG_INTERP_NN);
                    m_preview_win1->showImage(auxImg);
                }
                else
                    m_preview_win1->showImage(img4gui);
            }

            // Right from a stereo pair:
            if (m_preview_win2 && m_preview_win2->isOpen() && stObs &&
                stObs->hasImageRight && img4gui.getWidth() > 0)
            {
                // Apply image reduction?
                if (m_preview_reduction >= 2)
                {
                    unsigned int w = img4guiR.getWidth();
                    unsigned int h = img4guiR.getHeight();
                    CImage auxImg;
                    img4guiR.scaleImage(
                        auxImg, w / m_preview_reduction,
                        h / m_preview_reduction, IMG_INTERP_NN);
                    m_preview_win2->showImage(auxImg);
                }
                else
                    m_preview_win2->showImage(img4guiR);
            }

            // Disparity from a stereo pair:
            if (m_preview_win2 && m_preview_win2->isOpen() && stObs &&
                stObs->hasImageDisparity)
            {
                // Apply image reduction?
                if (m_preview_reduction >= 2)
                {
                    unsigned int w = stObs->imageDisparity.getWidth();
                    unsigned int h = stObs->imageDisparity.getHeight();
                    CImage auxImg;
                    stObs->imageDisparity.scaleImage(
                        auxImg, w / m_preview_reduction,
                        h / m_preview_reduction, IMG_INTERP_NN);
                    m_preview_win2->showImage(auxImg);
                }
                else
                    m_preview_win2->showImage(stObs->imageDisparity);
            }
        }
    }  // end show preview

    if (delayed_insertion_in_obs_queue)
    {
        if (m_cap_duo3d && m_cap_duo3d->captureIMUIsSet() && obsIMU)
            out_obs.push_back(CObservation::Ptr(obsIMU));
    }
    else
    {
        if (stObs) out_obs.push_back(CObservation::Ptr(stObs));
        if (obs) out_obs.push_back(CObservation::Ptr(obs));
        if (obs3D) out_obs.push_back(CObservation::Ptr(obs3D));
    }
    return;
}

/* -----------------------------------------------------
                doProcess
----------------------------------------------------- */
void CCameraSensor::doProcess()
{
    vector<CSerializable::Ptr> out_obs;
    getNextFrame(out_obs);
    appendObservations(out_obs);
}

/* -----------------------------------------------------
                setSoftwareTriggerLevel
----------------------------------------------------- */
void CCameraSensor::setSoftwareTriggerLevel(bool level)
{
    if (m_cap_dc1394)
    {
        if (!m_cap_dc1394->setSoftwareTriggerLevel(level))
        {  // Error
            m_state = CGenericSensor::ssError;
            THROW_EXCEPTION("Error setting Trigger level by software");
        }
    }
    else
    {
        THROW_EXCEPTION(
            "Software trigger is not implemented for this camera type");
    }
}

/* -----------------------------------------------------
                setPathForExternalImages
----------------------------------------------------- */
void CCameraSensor::setPathForExternalImages(const std::string& directory)
{
    if (!mrpt::system::createDirectory(directory))
    {
        THROW_EXCEPTION_FMT(
            "Cannot create the directory for externally saved images: `%s`",
            directory.c_str());
    }
    m_path_for_external_images = directory;
}

/* ------------------------------------------------------------------------
                        prepareVideoSourceFromUserSelection
   ------------------------------------------------------------------------ */
CCameraSensor::Ptr mrpt::hwdrivers::prepareVideoSourceFromUserSelection()
{
#if MRPT_HAS_WXWIDGETS
    // Create the main wxThread, if it doesn't exist yet:
    if (!mrpt::gui::WxSubsystem::createOneInstanceMainThread())
    {
        std::cerr << "[mrpt::hwdrivers::prepareVideoSourceFromUserSelection] "
                     "Error initiating Wx subsystem."
                  << std::endl;
        return CCameraSensor::Ptr();  // Error!
    }

    std::promise<void> semDlg;
    std::promise<mrpt::gui::detail::TReturnAskUserOpenCamera> dlgSelection;

    // Create window:
    auto* REQ = new WxSubsystem::TRequestToWxMainThread[1];
    REQ->OPCODE = 700;
    REQ->sourceCameraSelectDialog = true;
    REQ->voidPtr = reinterpret_cast<void*>(&semDlg);
    REQ->voidPtr2 = reinterpret_cast<void*>(&dlgSelection);
    WxSubsystem::pushPendingWxRequest(REQ);

    // Wait for the window to realize and signal it's alive:
    if (!WxSubsystem::isConsoleApp())
    {
        std::this_thread::sleep_for(
            20ms);  // Force at least 1-2 timer ticks for processing the event:
        wxApp::GetInstance()->Yield(true);
    }

    // wait for window construction:
    int maxTimeout =
#ifdef _DEBUG
        30000;
#else
        6000;
#endif
    // If we have an "MRPT_WXSUBSYS_TIMEOUT_MS" environment variable, use that
    // timeout instead:
    const char* envVal = getenv("MRPT_WXSUBSYS_TIMEOUT_MS");
    if (envVal) maxTimeout = atoi(envVal);

    if (semDlg.get_future().wait_for(std::chrono::milliseconds(maxTimeout)) ==
        std::future_status::timeout)
    {
        cerr << "[prepareVideoSourceFromUserSelection] Timeout waiting window "
                "creation."
             << endl;
        return CCameraSensor::Ptr();
    }

    // wait for user selection:
    auto future = dlgSelection.get_future();
    future.wait();
    const auto& ret = future.get();

    // If the user didn't accept the dialog, return now:
    if (!ret.accepted_by_user) return CCameraSensor::Ptr();

    mrpt::config::CConfigFileMemory selectedConfig(ret.selectedConfig);

    CCameraSensor::Ptr cam = std::make_shared<CCameraSensor>();
    cam->loadConfig(selectedConfig, "CONFIG");
    cam->initialize();  // This will raise an exception if neccesary

    return cam;
#else
    THROW_EXCEPTION("MRPT compiled without wxWidgets");
#endif  // MRPT_HAS_WXWIDGETS
}

/* ------------------------------------------------------------------------
                        prepareVideoSourceFromPanel
   ------------------------------------------------------------------------ */
CCameraSensor::Ptr mrpt::hwdrivers::prepareVideoSourceFromPanel(void* _panel)
{
#if MRPT_HAS_WXWIDGETS

    try
    {
        CConfigFileMemory cfg;
        writeConfigFromVideoSourcePanel(_panel, "CONFIG", &cfg);

        // Try to open the camera:
        CCameraSensor::Ptr video = std::make_shared<CCameraSensor>();
        video->loadConfig(cfg, "CONFIG");

        // This will raise an exception if neccesary
        video->initialize();

        return video;
    }
    catch (const std::exception& e)
    {
        cerr << endl << e.what() << endl;
        wxMessageBox(_("Couldn't open video source"), _("Error"));
        return CCameraSensor::Ptr();
    }
#else
    THROW_EXCEPTION("MRPT compiled without wxWidgets");
#endif  // MRPT_HAS_WXWIDGETS
}

/* ------------------------------------------------------------------------
                        writeConfigFromVideoSourcePanel
   ------------------------------------------------------------------------ */
void mrpt::hwdrivers::writeConfigFromVideoSourcePanel(
    void* _panel, const std::string& sect, mrpt::config::CConfigFileBase* cfg)
{
    MRPT_START
#if MRPT_HAS_WXWIDGETS
    ASSERT_(_panel);
    auto* panel = reinterpret_cast<mrpt::gui::CPanelCameraSelection*>(_panel);
    ASSERTMSG_(
        panel, "panel must be of type mrpt::gui::CPanelCameraSelection *");
    panel->writeConfigFromVideoSourcePanel(sect, cfg);

#else
    THROW_EXCEPTION("MRPT compiled without wxWidgets");
#endif  // MRPT_HAS_WXWIDGETS
    MRPT_END
}

/* ------------------------------------------------------------------------
                        readConfigIntoVideoSourcePanel
   ------------------------------------------------------------------------ */
void mrpt::hwdrivers::readConfigIntoVideoSourcePanel(
    void* _panel, const std::string& sect,
    const mrpt::config::CConfigFileBase* cfg)
{
    MRPT_START
#if MRPT_HAS_WXWIDGETS
    ASSERT_(_panel);
    auto* panel = reinterpret_cast<mrpt::gui::CPanelCameraSelection*>(_panel);
    ASSERTMSG_(
        panel, "panel must be of type mrpt::gui::CPanelCameraSelection *");

    panel->readConfigIntoVideoSourcePanel(sect, cfg);

#else
    THROW_EXCEPTION("MRPT compiled without wxWidgets");
#endif  // MRPT_HAS_WXWIDGETS
    MRPT_END
}

/* -----------------------------------------------------
        THREAD: Saver of external images
   ----------------------------------------------------- */
void CCameraSensor::thread_save_images(unsigned int my_working_thread_index)
{
    while (!m_threadImagesSaverShouldEnd)
    {
        TListObservations newObs;

        // is there any new image?
        m_csToSaveList.lock();
        m_toSaveList[my_working_thread_index].swap(newObs);
        m_csToSaveList.unlock();

        for (auto i = newObs.begin(); i != newObs.end(); ++i)
        {
            // Optional user-code hook:
            if (m_hook_pre_save)
            {
                if (IS_DERIVED(*i->second, CObservation))
                {
                    mrpt::obs::CObservation::Ptr obs =
                        std::dynamic_pointer_cast<mrpt::obs::CObservation>(
                            i->second);
                    m_hook_pre_save(obs, m_hook_pre_save_param);
                }
            }

            if (IS_CLASS(*i->second, CObservationImage))
            {
                CObservationImage::Ptr obs =
                    std::dynamic_pointer_cast<CObservationImage>(i->second);

                string filName =
                    fileNameStripInvalidChars(trim(m_sensorLabel)) +
                    format(
                        "_%f.%s", (double)timestampTotime_t(obs->timestamp),
                        m_external_images_format.c_str());

                obs->image.saveToFile(
                    m_path_for_external_images + string("/") + filName,
                    m_external_images_jpeg_quality);
                obs->image.setExternalStorage(filName);
            }
            else if (IS_CLASS(*i->second, CObservationStereoImages))
            {
                CObservationStereoImages::Ptr stObs =
                    std::dynamic_pointer_cast<CObservationStereoImages>(
                        i->second);

                const string filNameL =
                    fileNameStripInvalidChars(trim(m_sensorLabel)) +
                    format(
                        "_L_%f.%s", (double)timestampTotime_t(stObs->timestamp),
                        m_external_images_format.c_str());
                const string filNameR =
                    fileNameStripInvalidChars(trim(m_sensorLabel)) +
                    format(
                        "_R_%f.%s", (double)timestampTotime_t(stObs->timestamp),
                        m_external_images_format.c_str());
                const string filNameD =
                    fileNameStripInvalidChars(trim(m_sensorLabel)) +
                    format(
                        "_D_%f.%s", (double)timestampTotime_t(stObs->timestamp),
                        m_external_images_format.c_str());

                stObs->imageLeft.saveToFile(
                    m_path_for_external_images + string("/") + filNameL,
                    m_external_images_jpeg_quality);
                stObs->imageLeft.setExternalStorage(filNameL);

                if (stObs->hasImageRight)
                {
                    stObs->imageRight.saveToFile(
                        m_path_for_external_images + string("/") + filNameR,
                        m_external_images_jpeg_quality);
                    stObs->imageRight.setExternalStorage(filNameR);
                }
                if (stObs->hasImageDisparity)
                {
                    stObs->imageDisparity.saveToFile(
                        m_path_for_external_images + string("/") + filNameD,
                        m_external_images_jpeg_quality);
                    stObs->imageDisparity.setExternalStorage(filNameD);
                }
            }

            // Append now:
            appendObservation(i->second);
        }

        std::this_thread::sleep_for(2ms);
    }
}