ImageToArMem.cpp

Go to the documentation of this file.

#include "ImageToArMem.h"
 
#include <sstream>
#include <unordered_set>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/observers/variant/Variant.h>
 
#include <RobotAPI/libraries/armem/client.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
 
#include <VisionX/libraries/armem/vision/camera/client/constants.h>
#include <VisionX/libraries/armem/vision/camera/core/aron/CameraCalibration.aron.generated.h>
#include <VisionX/libraries/armem/vision/camera/core/ice_conversions.h>
#include <VisionX/tools/ImageUtil.h>
 
#include <Image/ByteImage.h>
#include <Image/ImageProcessor.h>
 
namespace visionx::armem_images
{
 
    ImageToArMem::ImageToArMem() : ImageAdapter("ImageToArMem")
    {
    }
 
    template <class AronImageT>
    void
    ImageToArMem::addImages(const armarx::armem::MemoryID& entityID,
                            const std::vector<size_t>& imageIndices)
    {
        auto& images = imagesByEntity[entityID];
 
        for (size_t i = 0; i < imageIndices.size(); ++i)
        {
            armarx::armem::MemoryID instanceID = entityID;
            instanceID.instanceIndex = static_cast<int>(i);
 
            size_t index = imageIndices[i];
            images.emplace_back(std::make_unique<Image<AronImageT>>(instanceID, index));
        }
    }
 
    void
    ImageToArMem::setWritingMemory(armarx::armem::server::WritingMemoryInterfacePrx memory)
    {
        memoryWriter.setWritingMemory(memory);
    }
 
    void
    ImageToArMem::setWriter(const armarx::armem::client::Writer& writer)
    {
        memoryWriter = writer;
    }
 
    void
    ImageToArMem::addImagesRGB(const armarx::armem::MemoryID& entityID,
                               const std::vector<size_t>& imageIndices)
    {
        if (imageIndices.empty())
        {
            return;
        }
 
        addImages<arondto::ImageRGB>(entityID, imageIndices);
        for (const auto& image : imagesByEntity.at(entityID))
        {
            ARMARX_CHECK_EQUAL(image->getImage().type(), CV_8UC3);
        }
    }
 
    void
    ImageToArMem::addImagesDepth(const armarx::armem::MemoryID& entityID,
                                 const std::vector<size_t>& imageIndices)
    {
        if (imageIndices.empty())
        {
            return;
        }
 
        addImages<arondto::ImageDepth>(entityID, imageIndices);
        for (const auto& image : imagesByEntity.at(entityID))
        {
            ARMARX_CHECK_EQUAL(image->getImage().type(), CV_32FC1);
        }
    }
 
    armarx::armem::MemoryID
    ImageToArMem::getMemoryID() const
    {
        auto it = imagesByEntity.begin();
        armarx::armem::MemoryID id = it->first;
        for (; it != imagesByEntity.end(); ++it)
        {
            ARMARX_CHECK_EQUAL(it->first.memoryName, id.memoryName)
                << "Currently, all images must be from the same memory.";
 
            ARMARX_CHECK_EQUAL(it->first.providerSegmentName, id.providerSegmentName)
                << "Currently, all images must be from the same provider.";
        }
        return id;
    }
 
    armarx::armem::data::AddSegmentsInput
    ImageToArMem::makeAddSegmentsInput(bool clearWhenExists)
    {
        std::unordered_set<armarx::armem::MemoryID> providerIDs;
        for (const auto& [entityID, image] : imagesByEntity)
        {
            providerIDs.insert(entityID.getProviderSegmentID());
        }
        armarx::armem::data::AddSegmentsInput inputs;
        for (const armarx::armem::MemoryID& id : providerIDs)
        {
            armarx::armem::data::AddSegmentInput& input = inputs.emplace_back();
            input.coreSegmentName = id.coreSegmentName;
            input.providerSegmentName = id.providerSegmentName;
            input.clearWhenExists = clearWhenExists;
        }
        return inputs;
    }
 
    armarx::armem::data::AddSegmentsResult
    ImageToArMem::addProviderSegments(bool clearWhenExists, int verbose)
    {
        const armarx::armem::data::AddSegmentsInput inputs = makeAddSegmentsInput(clearWhenExists);
        const armarx::armem::data::AddSegmentsResult results = memoryWriter.addSegments(inputs);
        if (verbose >= 1)
        {
            for (size_t i = 0; i < results.size(); ++i)
            {
                if (!results.at(i).success)
                {
                    const armarx::armem::data::AddSegmentInput& in = inputs.at(i);
                    ARMARX_ERROR << "Failed to add provider segment '" << in.coreSegmentName << "/"
                                 << in.providerSegmentName << "'."
                                 << "\n"
                                 << results.at(0).errorMessage;
                }
            }
        }
        return results;
    }
 
    void
    ImageToArMem::useImageBuffers(CByteImage** inputImageBuffer, armarx::armem::Time timeProvided)
    {
        for (auto& [entityID, images] : imagesByEntity)
        {
            for (std::unique_ptr<ImageBase>& image : images)
            {
                image->usePixels(inputImageBuffer);
            }
        }
        this->timestamp = timeProvided;
    }
 
    void
    ImageToArMem::usePixelBuffers(void** inputImageBuffers, armarx::armem::Time timeProvided)
    {
        for (auto& [entityID, images] : imagesByEntity)
        {
            for (std::unique_ptr<ImageBase>& image : images)
            {
                image->usePixels(inputImageBuffers);
            }
        }
        this->timestamp = timeProvided;
    }
 
    armarx::armem::Commit
    ImageToArMem::makeCommit() const
    {
        armarx::armem::Commit commit;
        for (const auto& [entityID, images] : imagesByEntity)
        {
            if (images.size())
            {
                armarx::armem::EntityUpdate& update = commit.updates.emplace_back();
                update.entityID = entityID;
                update.referencedTime = timestamp;
                for (const auto& image : images)
                {
                    update.instancesData.push_back(image->toAron());
                }
            }
        }
        return commit;
    }
 
    void
    ImageToArMem::commitImages()
    {
        ARMARX_CHECK(memoryWriter);
        ARMARX_DEBUG << "Got a new image for processing. Sending images to vision memory.";
 
        TIMING_START(ToAron);
        armarx::armem::Commit commit = makeCommit();
        TIMING_END_STREAM(ToAron, ARMARX_VERBOSE);
 
        TIMING_START(Commit);
        armarx::armem::CommitResult results = memoryWriter.commit(commit);
        TIMING_END_STREAM(Commit, ARMARX_VERBOSE);
 
        ARMARX_CHECK_EQUAL(results.results.size(), commit.updates.size());
        {
            std::stringstream ss;
            for (size_t i = 0; i < results.results.size(); ++i)
            {
                ss << "\n- " << commit.updates.at(i).entityID << ": \n" << results.results.at(i);
            }
            ARMARX_DEBUG << "Commit results: " << ss.str();
        }
 
        if (debugObserver)
        {
            debugObserver->setDebugChannel(
                debugObserverChannelName,
                {
                    {"ToAron [us]", new armarx::Variant(ToAron.toMicroSecondsDouble())},
                    {"Commit [us]", new armarx::Variant(Commit.toMicroSecondsDouble())},
                });
        }
    }
 
    void
    ImageToArMem::commitCameraCalibration(const visionx::StereoCalibration& calib)
    {
        visionx::arondto::StereoCameraCalibration aron;
        visionx::armem::camera::fromIce(aron, calib);
 
        armarx::armem::Commit c;
        armarx::armem::EntityUpdate& eu = c.add();
 
        eu.confidence = 1.0;
 
        eu.entityID = armarx::armem::MemoryID(
            getMemoryID().memoryName,
            visionx::armem::camera::STEREO_CAMERA_CALIBRATION_CORE_SEGMENT_NAME,
            getMemoryID().providerSegmentName,
            visionx::armem::camera::ENTITY_NAME);
 
        eu.instancesData = {aron.toAron()};
 
        eu.referencedTime = armarx::core::time::DateTime::Now();
        eu.sentTime = armarx::core::time::DateTime::Now();
        auto result = memoryWriter.commit(c);
 
        if (not result.allSuccess())
        {
            ARMARX_ERROR << "An error occured when sending pointclouds to the memory. "
                            "Try to continue with next PC. The message was: "
                         << result.allErrorMessages();
        }
    }
 
    void
    ImageToArMem::commitCameraCalibration(const visionx::MonocularCalibration& calib)
    {
        visionx::arondto::MonocularCameraCalibration aron;
        visionx::armem::camera::fromIce(aron, calib);
 
        armarx::armem::Commit c;
        armarx::armem::EntityUpdate& eu = c.add();
 
        eu.confidence = 1.0;
 
        eu.entityID = armarx::armem::MemoryID(
            getMemoryID().memoryName,
            visionx::armem::camera::MONOCULAR_CAMERA_CALIBRATION_CORE_SEGMENT_NAME,
            getMemoryID().providerSegmentName,
            visionx::armem::camera::ENTITY_NAME);
 
        eu.instancesData = {aron.toAron()};
 
        eu.referencedTime = armarx::core::time::DateTime::Now();
        eu.sentTime = armarx::core::time::DateTime::Now();
        auto result = memoryWriter.commit(c);
 
        if (not result.allSuccess())
        {
            ARMARX_ERROR << "An error occured when sending pointclouds to the memory. "
                            "Try to continue with next PC. The message was: "
                         << result.allErrorMessages();
        }
    }
 
    std::string
    ImageToArMem::summarizeStructure() const
    {
        std::stringstream ss;
        for (const auto& [entityID, images] : imagesByEntity)
        {
            ss << "- Entity " << entityID << ": \n";
            for (const auto& image : images)
            {
                ss << "- - [image #" << image->imageIndex << "] id = " << image->instanceID << "\n";
            }
        }
        return ss.str();
    }
 
    void
    ImageToArMem::initImages(int width, int height, CByteImage::ImageType type)
    {
        for (auto& [entityID, images] : imagesByEntity)
        {
            for (auto& image : images)
            {
                image->resetImage(height, width);
            }
        }
    }
 
    void
    ImageToArMem::initImages(const ImageFormatInfo& info)
    {
        for (auto& [entityID, images] : imagesByEntity)
        {
            for (auto& image : images)
            {
                image->resetImage(info.dimension.height, info.dimension.width);
            }
        }
    }
 
    std::vector<CByteImage>
    ImageToArMem::makeCByteImageBuffer()
    {
        std::vector<CByteImage> buffer;
        for (auto& [entityID, images] : imagesByEntity)
        {
            for (auto& image : images)
            {
                if (image->imageIndex >= buffer.size())
                {
                    buffer.resize(image->imageIndex + 1);
                }
                cv::Mat& mat = image->getImage();
                buffer[image->imageIndex].Set(mat.cols, mat.rows, CByteImage::eRGB24);
            }
        }
        return buffer;
    }
 
} // namespace visionx::armem_images