Component.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::ArmarXObjects::Yolo
 * @author     Christian R. G. Dreher <c.dreher@kit.edu>
 * @date       2019
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#include <VisionX/components/image_processor/Yolo/Component.h>
 
 
// STD/STL
#include <algorithm> // std::find, std::max, std::min
#include <chrono>
#include <fstream> // std::ifstream
#include <future>
#include <iostream> // std::fixed
#include <iomanip> // std::setprecision
#include <iterator> // std::begin, std::end, std::distance
#include <map>
#include <thread> // std::sleep_for
 
// IVT
#include <Image/ImageProcessor.h>
#include <Image/IplImageAdaptor.h>
#include <Image/PrimitivesDrawer.h>
#include <Image/PrimitivesDrawerCV.h> // Deprecated, but required to draw text.
 
// Ice
#include <IceUtil/Time.h>
 
// ArmarX
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/time/ScopedStopWatch.h>
#include <ArmarXCore/core/time/StopWatch.h>
 
 
using namespace armarx;
using namespace visionx;
using namespace visionx::yolo;
 
 
namespace
{
    IceUtil::Time timestamp_invalid = IceUtil::Time::milliSeconds(-1);
}
 
 
const std::string
yolo::Component::default_name = "Yolo";
 
 
yolo::Component::~Component()
{
    // pass
}
 
 
void
yolo::Component::onInitImageProcessor()
{
    // Initialise runtime parameters from defaults.
    restoreDefaults();
 
    m_image_received = false;
 
    // Signal dependency on image provider
    m_image_provider_id = getProperty<std::string>("ipc.ImageProviderName");
    ARMARX_VERBOSE << "Using image provider with ID '" << m_image_provider_id << "'.";
    usingImageProvider(m_image_provider_id);
 
    // Get data channel.
    m_image_provider_channel = getProperty<unsigned int>("ipc.ImageProviderChannel");
 
    // Topic name under which the detection results are published.
    {
        std::string topic_name = getProperty<std::string>("ipc.ObjectDetectionResultTopicName");
        offeringTopic(topic_name);
    }
 
    // Load known classes.
    {
        std::ifstream infile{getProperty<std::string>("darknet.namefile")};
        std::string line;
        while (std::getline(infile, line))
        {
            m_darknet_classes.push_back(line);
        }
 
        m_network.class_count(static_cast<int>(m_darknet_classes.size()));
    }
 
    // Boot Darknet. This may take several seconds to complete.
    bootstrap_darknet();
 
    ARMARX_INFO << "Darknet provider initialised.";
}
 
 
void
yolo::Component::onConnectImageProcessor()
{
    // Connect to image provider.
    m_image_provider_info = getImageProvider(m_image_provider_id);
    m_image_provider = getProxy<visionx::ImageProviderInterfacePrx>(m_image_provider_id);
 
    // Init input image.
    {
        const unsigned int num_images =
            static_cast<unsigned int>(m_image_provider_info.numberImages);
        m_input_image_buf = new ::CByteImage*[num_images];
        for (unsigned int i = 0; i < num_images; ++i)
        {
            m_input_image_buf[i] = visionx::tools::createByteImage(m_image_provider_info);
        }
        m_input_image.reset(visionx::tools::createByteImage(m_image_provider_info));
        m_output_image.reset(visionx::tools::createByteImage(m_image_provider_info));
    }
 
    // Topic of detected objects for consumers.
    std::string topic_name = getProperty<std::string>("ipc.ObjectDetectionResultTopicName");
    m_object_detection_listener = getTopic<ObjectListener::ProxyType>(topic_name);
 
    // Initialise visual output if enabled.
    if (getProperty<bool>("yolo.EnableVisualisation"))
    {
        const int num_images = 1;
        enableResultImages(
            num_images,
            m_image_provider_info.imageFormat.dimension,
            m_image_provider_info.imageFormat.type
        );
    }
 
    // Kick off running task
    m_task_object_detection = new RunningTask<yolo::Component>(
        this,
        &yolo::Component::run_object_detection_task);
    m_task_object_detection->start();
 
    ARMARX_INFO << "Darknet provider connected. Operation begins now.";
}
 
 
void
yolo::Component::onDisconnectImageProcessor()
{
    // Stop task.
    {
        const bool wait_for_join = true;
        m_task_object_detection->stop(wait_for_join);
    }
 
    // Clear input image buffer.
    {
        const unsigned int num_images =
            static_cast<unsigned int>(m_image_provider_info.numberImages);
        for (unsigned int i = 0; i < num_images; ++i)
        {
            delete m_input_image_buf[i];
        }
        delete[] m_input_image_buf;
    }
 
    ARMARX_INFO << "Darknet provider disconnected.";
}
 
 
void
yolo::Component::onExitImageProcessor()
{
    ARMARX_INFO << "Darknet provider uninitialised.";
}
 
 
std::string
yolo::Component::getDefaultName() const
{
    return yolo::Component::default_name;
}
 
 
void
yolo::Component::process()
{
    ARMARX_VERBOSE << "Processing frame...";
 
    {
        const IceUtil::Time timeout = IceUtil::Time::milliSeconds(1000);
        if (not waitForImages(m_image_provider_id, static_cast<int>(timeout.toMilliSeconds())))
        {
            ARMARX_WARNING << "Timeout while waiting for camera images (>" << timeout << ")";
            return;
        }
    }
 
    std::lock_guard<std::mutex> lock{m_input_image_mutex};
 
    MetaInfoSizeBasePtr info;
    int num_images = getImages(m_image_provider_id, m_input_image_buf, info);
    m_timestamp_last_image = IceUtil::Time::microSeconds(info->timeProvided);
 
    if (num_images >= 1)
    {
        // Only consider first image.
        ::ImageProcessor::CopyImage(m_input_image_buf[m_image_provider_channel],
                                    m_input_image.get());
        m_image_received = true;
    }
    else
    {
        ARMARX_WARNING << "Didn't receive an image.";
    }
}
 
 
double
yolo::Component::getThresh(const Ice::Current&)
{
    return static_cast<double>(m_network.thresh());
}
 
 
void
yolo::Component::setThresh(double thresh, const Ice::Current&)
{
    m_network.thresh(static_cast<float>(thresh));
    ARMARX_VERBOSE << "Parameter thresh is now " << thresh << ".";
}
 
 
double
yolo::Component::getHierThresh(const Ice::Current&)
{
    return static_cast<double>(m_network.hier_thresh());
}
 
 
void
yolo::Component::setHierThresh(double hier_thresh, const Ice::Current&)
{
    m_network.hier_thresh(static_cast<float>(hier_thresh));
    ARMARX_VERBOSE << "Parameter hier_thresh is now " << hier_thresh << ".";
}
 
 
double
yolo::Component::getNms(const Ice::Current&)
{
    return static_cast<double>(m_network.nms());
}
 
 
void
yolo::Component::setNms(double nms, const Ice::Current&)
{
    m_network.nms(static_cast<float>(nms));
    ARMARX_VERBOSE << "Parameter nms is now " << nms << ".";
}
 
 
float
yolo::Component::getFpsCap(const Ice::Current&)
{
    // If FPS cap is disabled, always return -1 for a consistent API.
    if (m_minimum_loop_time == ::timestamp_invalid)
    {
        return -1.;
    }
 
    // Derive the FPS cap from the minimum loop time.
    return 1000.f / m_minimum_loop_time.toMilliSeconds();
}
 
 
void
yolo::Component::setFpsCap(float fps_cap, const Ice::Current&)
{
    // Set m_minimum_loop_time to zero for all values for fps_cap <= 0 to disable the cap.
    if (fps_cap <= 0)
    {
        m_minimum_loop_time = ::timestamp_invalid;
        return;
    }
 
    // Don't actually store the fps cap, but calculate the minimum loop time in [ms].
    m_minimum_loop_time = IceUtil::Time::milliSecondsDouble(1000. / static_cast<double>(fps_cap));
}
 
 
std::vector<std::string>
yolo::Component::getClasses(const Ice::Current&)
{
    return m_darknet_classes;
}
 
 
void
yolo::Component::restoreDefaults(const Ice::Current&)
{
    ARMARX_INFO << "Restoring default parameter values.";
    m_network.thresh(static_cast<float>(getProperty<double>("darknet.thresh")));
    m_network.hier_thresh(static_cast<float>(getProperty<double>("darknet.hier_thresh")));
    m_network.nms(static_cast<float>(getProperty<double>("darknet.nms")));
    setFpsCap(getProperty<float>("yolo.FPSCap"));
}
 
 
void
yolo::Component::bootstrap_darknet()
{
    ARMARX_INFO << "Bootstrapping Darknet.";
 
    ScopedStopWatch sw{[&](IceUtil::Time time)
    {
        ARMARX_INFO << std::fixed << std::setprecision(3) << "Bootstrapped Darknet in "
                    << time.toSecondsDouble() << " seconds.";
    }};
 
    // Get properties.
    const std::string cfgfile = getProperty<std::string>("darknet.cfgfile");
    const std::string weightfile = getProperty<std::string>("darknet.weightfile");
 
    // Bootstrap Darknet.
    m_network.load(cfgfile, weightfile);
    m_network.set_batch(1);
}
 
 
void
yolo::Component::run_object_detection_task()
{
    // Initialise working copy instance of input_image.
    CByteImageUPtr input_image;
    {
        visionx::ImageProviderInfo image_provider_info = getImageProvider(m_image_provider_id);
        input_image.reset(visionx::tools::createByteImage(image_provider_info));
    }
 
    // Stop watch to assess timings.
    StopWatch sw;
 
    // Kick off detection loop.
    while (not m_task_object_detection->isStopped())
    {
        // Assess loop start time.
        sw.reset();
 
        ARMARX_VERBOSE << "Running Darknet.";
 
        IceUtil::Time timestamp_last_image;
 
        ARMARX_VERBOSE << "Creating local working copies.";
        {
            std::lock_guard<std::mutex> lock{m_input_image_mutex};
            if (not m_image_received)
            {
                ARMARX_VERBOSE << "No image provided yet, waiting...";
                continue;
            }
            timestamp_last_image = m_timestamp_last_image;
            const bool ok = ::ImageProcessor::CopyImage(m_input_image.get(), input_image.get());
            // Skip frame if copying fails (usually when buffer is empty at the beginning).
            if (not ok)
            {
                continue;
            }
            m_image_received = false;
        }
 
        ARMARX_VERBOSE << "Announcing which frame will be fed to Darknet.";
        m_object_detection_listener->announceDetectedObjects(timestamp_last_image.toMicroSeconds());
 
        // Have Darknet make preditions and convert to VisionX interface type. This call will
        // usually consume a considerable amount of time.
        std::vector<DetectedObject> detected_objects;
        {
            ::IplImage* input_image_conv =
                ::IplImageAdaptor::Adapt(input_image.get());  // Shallow copy.
            ARMARX_VERBOSE << "Predicting now...";
            detected_objects = convertOutput(m_network.predict(*input_image_conv),
                                             m_darknet_classes);
            ::cvReleaseImageHeader(&input_image_conv);  // Only header must be freed.
            ARMARX_VERBOSE << "Found " << detected_objects.size() << " objects.";
        }
 
        ARMARX_VERBOSE << "Broadcasting all detections.";
        m_object_detection_listener->reportDetectedObjects(
            detected_objects,
            timestamp_last_image.toMicroSeconds()
        );
 
        // Render output bounding boxes if visualisation is enabled.
        if (getProperty<bool>("yolo.EnableVisualisation"))
        {
            ARMARX_VERBOSE << "Rendering output bounding boxes.";
            yolo::Component::renderOutput(input_image.get(),
                                          detected_objects,
                                          m_output_image.get());
            ::CByteImage* output_images[1] = {m_output_image.get()};
            // Provide the images with the original timestamp.
            resultImageProvider->provideResultImages(
                output_images,
                timestamp_last_image.toMicroSeconds()
            );
        }
 
        // Assess timings.
        const IceUtil::Time loop_duration = sw.stop();
 
        ARMARX_VERBOSE << "Ran Darknet in " << loop_duration << ".";
 
        // If the cap is enabled and if the loop duration is less than the minimum loop time
        // => sleep for the difference in [ms].
        if (m_minimum_loop_time != ::timestamp_invalid and loop_duration < m_minimum_loop_time)
        {
            const IceUtil::Time time_delta = m_minimum_loop_time - loop_duration;
            ARMARX_VERBOSE << "FPS cap enabled, suspending thead for " << time_delta << ".";
            std::this_thread::sleep_for(std::chrono::milliseconds{time_delta.toMilliSeconds()});
        }
    }
 
    ARMARX_INFO << "Detection loop properly shut down.";
}
 
 
std::vector<DetectedObject>
yolo::Component::convertOutput(
    const std::vector<darknet::detection>& detections,
    const std::vector<std::string>& classes)
{
    std::vector<DetectedObject> detections_conv;
 
    for (const darknet::detection& detection : detections)
    {
        DetectedObject detection_conv;
        const int class_count = static_cast<int>(classes.size());
        detection_conv.classCount = class_count;
        detection_conv.boundingBox.x = detection.x();
        detection_conv.boundingBox.y = detection.y();
        detection_conv.boundingBox.w = detection.w();
        detection_conv.boundingBox.h = detection.h();
 
        for (const auto& [class_index, certainty] : detection.candidates())
        {
            ClassCandidate candidate_conv;
            candidate_conv.classIndex = class_index;
            candidate_conv.certainty = certainty;
            candidate_conv.className = classes.at(static_cast<unsigned int>(class_index));
 
            DrawColor24Bit color;
            std::tie(color.r, color.g, color.b) =
                darknet::get_color(candidate_conv.classIndex, class_count);
            candidate_conv.color = color;
 
            detection_conv.candidates.push_back(candidate_conv);
        }
 
        detections_conv.push_back(detection_conv);
    }
 
    return detections_conv;
}
 
 
void
yolo::Component::renderOutput(
    const ::CByteImage* input_image,
    const std::vector<DetectedObject>& detected_objects,
    ::CByteImage* output_image)
{
    ARMARX_VERBOSE_S << "Rendering output.";
 
    ::ImageProcessor::CopyImage(input_image, output_image);
 
    // Iterate over each detected object.
    for (const DetectedObject& detected_object : detected_objects)
    {
        const BoundingBox2D& bounding_box = detected_object.boundingBox;
        const ClassCandidate& class_candidate = *std::max_element(
                detected_object.candidates.begin(),
                detected_object.candidates.end(),
                [](const ClassCandidate & c1, const ClassCandidate & c2) -> bool
        {
            return c1.certainty < c2.certainty;
        });
 
        const DrawColor24Bit& color = class_candidate.color;
 
        // Draw bounding box.
        {
            const float angle = 0;
            const ::Rectangle2d rectangle
            {
                ::Vec2d{
                    bounding_box.x* input_image->width,
                    bounding_box.y* input_image->height
                },  // center.
                bounding_box.w* input_image->width,  // width.
                bounding_box.h* input_image->height,  // height.
                angle
            };
            const int thickness = 1;
            ::PrimitivesDrawer::DrawRectangle(
                output_image,
                rectangle,
                color.r, color.g, color.b,
                thickness
            );
        }
 
        // Draw class label.
        {
            const std::string object_instance_name = detected_object.objectName == "" ?
                    class_candidate.className : detected_object.objectName;
            const double text_scale = .4;
            const int text_thickness = 1;
            const float left =
                std::max((bounding_box.x - bounding_box.w / 2.f) * input_image->width, 0.f);
            const float top =
                std::max((bounding_box.y - bounding_box.h / 2.f) * input_image->height, 0.f);
            const double text_pos_left = static_cast<double>(left + 2);
            const double text_pos_top = static_cast<double>(top + (top < 10 ? 11 : 1));
 
            // Blurry thick text in bbox color as visual hint.
            ::PrimitivesDrawerCV::PutText(
                output_image,
                object_instance_name.c_str(),
                text_pos_left, text_pos_top,
                text_scale, text_scale,
                color.r, color.g, color.b,
                text_thickness + 5
            );
 
            // Slightly blurry white background (for good constrast with black class name label).
            ::PrimitivesDrawerCV::PutText(
                output_image,
                object_instance_name.c_str(),
                text_pos_left, text_pos_top,
                text_scale, text_scale,
                255, 255, 255, // white.
                text_thickness + 3
            );
 
            // Sharp, actually readable class name in black.
            ::PrimitivesDrawerCV::PutText(
                output_image,
                object_instance_name.c_str(),
                text_pos_left, text_pos_top,
                text_scale, text_scale,
                0, 0, 0, // black.
                text_thickness
            );
        }
    }
}
 
 
PropertyDefinitionsPtr
yolo::Component::createPropertyDefinitions()
{
    PropertyDefinitionsPtr defs{new ComponentPropertyDefinitions{getConfigIdentifier()}};
 
    // Options for inter process communication.
    defs->defineOptionalProperty<std::string>(
        "ipc.ObjectDetectionResultTopicName",
        "YoloDetectionResult",
        "Topic name for the object detection result"
    );
    defs->defineRequiredProperty<std::string>(
        "ipc.ImageProviderName",
        "Image provider as data source for Darknet"
    );
    defs->defineOptionalProperty<unsigned int>(
        "ipc.ImageProviderChannel",
        0,
        "Channel of the image provider with RGB images to consider"
    );
 
    // Options of the Yolo component.
    defs->defineOptionalProperty<bool>(
        "yolo.EnableVisualisation",
        true,
        "If set to true, a visualisation of the output will be provided under the topic defined in "
        "`ipc.ObjectDetectionResultTopicName`"
    );
    defs->defineOptionalProperty<float>(
        "yolo.FPSCap",
        -1.,
        "Cap the main loop to a fixed FPS rate to save resources.  Values <= 0 disable the cap, so "
        "that only the hardware limits the FPS"
    );
 
    // Options of Darknet.
    defs->defineOptionalProperty<double>(
        "darknet.thresh", .5,
        "Default thresh.  May be overridden at runtime"
    );
    defs->defineOptionalProperty<double>(
        "darknet.hier_thresh",
        .5,
        "Default hier_thresh.  May be overridden at runtime"
    );
    defs->defineOptionalProperty<double>(
        "darknet.nms",
        .45,
        "Default nms.  May be overridden at runtime"
    );
    defs->defineRequiredProperty<std::string>(
        "darknet.cfgfile",
        "Darknet configuration file"
    );
    defs->defineRequiredProperty<std::string>(
        "darknet.weightfile",
        "Darknet weight file"
    );
    defs->defineRequiredProperty<std::string>(
        "darknet.namefile",
        "File where the classes are listed (usually a '*.names' file in Darknet's data-directory)"
    );
 
    return defs;
}