PlaybackImageProvider.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    visionx::components
 * @author     Christian R. G. Dreher <c.dreher@kit.edu>
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#include <VisionX/components/image_provider/PlaybackImageProvider/PlaybackImageProvider.h>
#include <VisionX/libraries/armem/vision/images/server/plugins.h>
 
 
// ArmarX
#include <filesystem>
#include <functional>
#include <string>
#include <tuple>
#include <vector>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/time/TimeUtil.h>
 
#include <VisionX/components/image_provider/StereoCameraProvider/LoadingCalibrationFileFailedException.h>
#include <VisionX/components/image_provider/StereoCameraProvider/StereoCameraSystemRequiredException.h>
#include <VisionX/tools/TypeMapping.h>
 
using namespace armarx;
using namespace visionx;
using namespace visionx::components;
using visionx::exceptions::local::LoadingCalibrationFileFailedException;
using visionx::exceptions::local::StereoCameraSystemRequiredException;
using visionx::imrec::Playback;
 
PlaybackImageProvider::PlaybackImageProvider()
{
    m_stereo_initialized = false;
}
 
PlaybackImageProvider::~PlaybackImageProvider()
{
    // pass
}
 
namespace fs = std::filesystem;
 
void
PlaybackImageProvider::onInitCapturingImageProvider()
{
    getProperty(m_stereo_reference_frame, "stereo.reference_frame");
    getProperty(m_stereo_calibration_file, "stereo.calibration_file");
 
    fs::path basePath = getProperty<std::string>("base_path").getValue();
    std::string recordingFilesStr = getProperty<std::string>("recording_files").getValue();
    std::vector<std::string> recordingFiles = simox::alg::split(recordingFilesStr, ";");
 
    if (m_stereo_calibration_file != "" or m_stereo_reference_frame != "")
    {
        if (recordingFiles.size() != 2)
        {
            throw LocalException() << "Stereo calibration file and/or reference frame were set, "
                                   << "but there are " << recordingFiles.size() << " recording "
                                   << "files (expected 2).";
        }
 
        init_stereo_calibration_file();
        init_stereo_calibration();
    }
 
    auto parse_fps = [](const std::string& setting, double source_fps) -> std::tuple<double, double>
    {
        // This lambda assumes that all operations are safe to call, as fps_setting was checked
        // against a RegEx while configuration, and does therefore not perform any sanity checks
 
        const unsigned long int prefix_length = std::string("sourceX").size();
        std::string setting_value;
        std::function<double(double, double)> operation;
 
        // Source FPS
        if (setting == "source")
        {
            return std::make_tuple(source_fps, 1.);
        }
        // Source FPS multiplied by value
        else if (simox::alg::starts_with(setting, "source*"))
        {
            // Multiplication
            operation = [](double x, double y) -> double { return x * y; };
            setting_value = setting.substr(prefix_length);
        }
        // Source FPS divided by value
        else if (simox::alg::starts_with(setting, "source/"))
        {
            // Division
            operation = [](double x, double y) -> double { return x / y; };
            setting_value = setting.substr(prefix_length);
        }
        // Source FPS is ignored and the value is assumed to be source FPS
        else if (simox::alg::starts_with(setting, "source="))
        {
            return std::make_tuple(std::stod(setting.substr(prefix_length)), 1.);
        }
        // Set FPS to a specific value
        else
        {
            // Identity to keep the user defined setting regardless of source FPS
            operation = [](double, double fps) -> double { return fps; };
            setting_value = setting;
        }
 
        const double derived_fps = operation(source_fps, std::stod(setting_value));
        const double playback_speed_normalisation = source_fps / derived_fps;
 
        return std::make_tuple(derived_fps, playback_speed_normalisation);
    };
 
    unsigned int refFrameHeight = 0;
    unsigned int refFrameWidth = 0;
    unsigned int actual_source_fps = 0;
 
    // Instantiate playbacks
    for (std::string const& recordingFile : recordingFiles)
    {
        fs::path fullPath = basePath != "" ? basePath / recordingFile : fs::path(recordingFile);
        Playback playback = imrec::newPlayback(fullPath);
        m_playbacks.push_back(playback);
 
        if (not playback)
        {
            ARMARX_ERROR << "Could not find a playback strategy for '" << fullPath.string() << "'";
            continue;
        }
 
        // Sanity checks for image heights and widths
        {
            // If uninitialised, use first playback as reference
            if (refFrameHeight == 0 and refFrameWidth == 0)
            {
                refFrameHeight = playback->getFrameHeight();
                refFrameWidth = playback->getFrameWidth();
            }
 
            // Check that neither hight nor width are 0 pixels
            ARMARX_CHECK_GREATER(playback->getFrameHeight(), 0)
                << "Image source frame height cannot be 0 pixel";
            ARMARX_CHECK_GREATER(playback->getFrameWidth(), 0)
                << "Image source frame width cannot be 0 pixel";
 
            // Check that the frame heights and widths from all image sources are equal
            ARMARX_CHECK_EQUAL(playback->getFrameHeight(), refFrameHeight)
                << "Image source frames must have the same dimensions";
            ARMARX_CHECK_EQUAL(playback->getFrameWidth(), refFrameWidth)
                << "Image source frames must have the same dimensions";
        }
 
        // Determine playback with the highest FPS
        if (playback->getFps() > actual_source_fps)
        {
            actual_source_fps = playback->getFps();
        }
    }
 
    // Figure out FPS related variables base FPS, normalisations, multiplier
    double playback_speed_normalisation;
    std::tie(frameRate, playback_speed_normalisation) =
        parse_fps(getProperty<std::string>("fps"), actual_source_fps);
    m_frame_advance =
        playback_speed_normalisation * getProperty<double>("playback_speed_multiplier");
 
    ARMARX_INFO << "Playback at " << frameRate << " FPS, advancing " << m_frame_advance
                << " per loop";
 
    // Set up required properties for the image capturer
    setNumberImages(static_cast<int>(m_playbacks.size()));
    setImageFormat(
        ImageDimension(static_cast<int>(refFrameWidth), static_cast<int>(refFrameHeight)),
        eRgb,
        eBayerPatternGr);
    setImageSyncMode(eFpsSynchronization);
}
 
void
PlaybackImageProvider::onExitCapturingImageProvider()
{
    for (Playback& playback : m_playbacks)
    {
        playback->stopPlayback();
    }
 
    m_playbacks.clear();
}
 
void
PlaybackImageProvider::onStartCapture(float)
{
    // pass
}
 
void
PlaybackImageProvider::onStopCapture()
{
    // pass
}
 
bool
PlaybackImageProvider::capture(void** imageBuffer)
{
    const bool loop = getProperty<bool>("loop");
    bool success = false; // Result is true if at least one of the playbacks provided a picture
 
    for (unsigned int i = 0; i < static_cast<unsigned int>(getNumberImages()); ++i)
    {
        Playback playback = m_playbacks[i];
 
        // If the last frame is reached and loop is enabled, rewind
        if (not playback->hasNextFrame() and loop)
        {
            m_current_frame = 0;
        }
 
        const unsigned int current_frame = static_cast<unsigned int>(std::round(m_current_frame));
        const unsigned int min_frame = 0;
        const unsigned int max_frame = playback->getFrameCount() - 1;
        unsigned int frame_index = std::clamp(current_frame, min_frame, max_frame);
        playback->setCurrentFrame(frame_index);
        success |= playback->getNextFrame(imageBuffer[i]);
    }
 
    m_current_frame += m_frame_advance;
 
    const armarx::DateTime time = armarx::DateTime::Now();
    if (success)
    {
        updateTimestamp(IceUtil::Time::microSeconds(time.toMicroSecondsSinceEpoch()));
    }
 
    // Update working memory.
    const bool memoryEnabled = getProperty<bool>("memory.enable");
    if (memoryEnabled and imageBuffer != nullptr)
    {
        getImagesServerPlugin().commitImages(imageBuffer, time);
    }
 
    return success;
}
 
StereoCalibration
PlaybackImageProvider::getStereoCalibration(const Ice::Current&)
{
    assert_stereo_initialized();
    return m_stereo_calibration;
}
 
bool
PlaybackImageProvider::getImagesAreUndistorted(const Ice::Current&)
{
    assert_stereo_initialized();
    return false;
}
 
std::string
PlaybackImageProvider::getReferenceFrame(const Ice::Current&)
{
    assert_stereo_initialized();
    return m_stereo_reference_frame;
}
 
std::string
PlaybackImageProvider::getDefaultName() const
{
    return "PlaybackImageProvider";
}
 
PropertyDefinitionsPtr
PlaybackImageProvider::createPropertyDefinitions()
{
    PropertyDefinitionsPtr defs{new ComponentPropertyDefinitions(getConfigIdentifier())};
 
    defs->defineRequiredProperty<std::string>(
        "recording_files",
        "List of recording files, separated by semicolons `;` for each channel.  For video files, "
        "use the filename, for image sequences, use the folder name where the image sequence is "
        "located or any frame as pattern.\n"
        "Wildcards (e.g. `frame_left_*.jpg;frame_right_*.jpg`) are supported as well.\n"
        "Files will be interpreted as absolute paths, or relative paths to the current working "
        "directory if base_path is not set. If base_path is set, all paths in recording_files are "
        "interpreted relative to base_path");
    defs->defineOptionalProperty<std::string>(
        "base_path",
        "",
        "Common base path of recording_files (will be prepened if set).  To unset, use \"\"");
    defs->defineOptionalProperty<double>("playback_speed_multiplier",
                                         1.0,
                                         "Adjust the playback speed with this multiplier, e.g. `2` "
                                         "= double playback speed, `0.5` = "
                                         "half playback speed")
        .setMin(0);
    defs->defineOptionalProperty<std::string>(
            "fps",
            "source",
            "Lock the FPS to an absolute value, or a value relative to source FPS.  Valid inputs:\n"
            "  1) `source` => Derive FPS from source\n"
            "  2) `source*<X>`, with <X> being a positive decimal or integer => Playback with FPS "
            "= "
            "source FPS multiplied by <X>\n"
            "  3) `source/<X>`, with <X> being a positive decimal or integer => Playback with FPS "
            "= "
            "source FPS devided by <X>\n"
            "  4) `<X>`, with <X> being a positive decimal or integer => Playback with FPS at <X>\n"
            "  5) `source=<X>`, with <X> being a positive decimal or integer => Playback with FPS "
            "at "
            "<X>, ignoring source FPS completely (Assume that <X> is source FPS)\n"
            "With the exception of 5), all settings only have direct effect of the FPS the image "
            "provider delivers the frames, but not on the playback speed. "
            "Use `playback_speed_multiplier` to adjust that.\n"
            "5) is only useful if the metadata of the recording is incomplete or incorrect (for "
            "example when replaying generic image sequences where the FPS cannot be derived)")
        .setMatchRegex(
            R"(source(\/|\*|=)(\d+(\.\d*)?|\.\d+)|(\d+(\.\d*)?|\.\d+)|(\d+(\.\d*)?|\.\d+)|source)");
    defs->defineOptionalProperty<bool>(
        "loop", true, "Whether the playback should restart after the last frame was provided");
 
    // Stereo functionality
    defs->defineOptionalProperty<std::string>(
        "stereo.calibration_file",
        "",
        "Path to a stereo calibration file that should additionally be provided.");
    defs->defineOptionalProperty<std::string>(
        "stereo.reference_frame",
        "",
        "Path to a stereo calibration file that should additionally be provided.");
 
 
    // Working Memory
    defs->defineOptionalProperty<bool>(
        "memory.enable", true, "If true, also provide the images as a memory server.");
 
    return defs;
}
 
void
PlaybackImageProvider::init_stereo_calibration_file()
{
    const bool success = armarx::ArmarXDataPath::getAbsolutePath(m_stereo_calibration_file,
                                                                 m_stereo_calibration_file);
 
    if (not success)
    {
        throw LoadingCalibrationFileFailedException(m_stereo_calibration_file.c_str());
    }
}
 
void
PlaybackImageProvider::init_stereo_calibration()
{
    const bool transform_left_camera_to_identity = true;
    const bool loading_calibration_sucessful = m_stereo_calibration_ivt.LoadCameraParameters(
        m_stereo_calibration_file.c_str(), transform_left_camera_to_identity);
 
    if (not loading_calibration_sucessful)
    {
        throw LoadingCalibrationFileFailedException(m_stereo_calibration_file.c_str());
    }
 
    m_stereo_calibration = visionx::tools::convert(m_stereo_calibration_ivt);
    m_stereo_initialized = true;
}
 
void
PlaybackImageProvider::assert_stereo_initialized()
{
    if (not m_stereo_initialized)
    {
        throw LocalException() << "Stereo calibration API was called, but the calibration of this "
                               << "PlaybackProvider (instance name '" << getName() << "') was "
                               << "never initialized.\n"
                               << "Provide this PlaybackProvider instance with a calibration file "
                               << "using the 'stereo.calibration_file' property.";
    }
}