ImageToArMem.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::SimpleEpisodicMemoryImageConnector
 * @author     Fabian Peller ( fabian dot peller-konrad at kit dot edu )
 * @date       2020
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "ImageToArMem.h"
 
// IVT
#include <ArmarXCore/observers/variant/Variant.h>
 
#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem/core/error.h>
 
#include <VisionX/tools/ImageUtil.h>
 
#include <Image/ImageProcessor.h>
 
namespace visionx
{
 
    std::string
    ImageToArMem::getDefaultName() const
    {
        return "ImageToArMem";
    }
 
    armarx::PropertyDefinitionsPtr
    ImageToArMem::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr defs(
            new armarx::ComponentPropertyDefinitions{getConfigIdentifier()});
 
        defs->topic(debugObserver);
 
        defs->optional(p.imageProviderName, "img.ProviderName", "Name of the image provier.");
        defs->optional(p.providerSegmentName,
                       "img.ProviderSegmentName",
                       "The provider (segment) name (if not specified in the entity ID).\n"
                       "If '(auto)', the image provider name is used.");
 
        defs->optional(p.clearProviderSegmentWhenExists,
                       "mem.ClearProviderSegmentsWhenExisting",
                       "If true, the provider segments are cleared when this component starts.");
 
        defs->optional(p.commitCameraCalib,
                       "mem.commitCameraCalibration",
                       "Commit camera calibration to memory");
 
        p.images.define(*defs);
 
        return defs;
    }
 
    void
    visionx::ImageToArMem::onInitImageProcessor()
    {
        // Parse properties.
        if (p.providerSegmentName == "(auto)")
        {
            p.providerSegmentName = p.imageProviderName;
        }
        p.images.read(*this, p.providerSegmentName);
 
        imageToArMem.addImagesRGB(p.images.rgbEntityID, p.images.rgbIndices);
        imageToArMem.addImagesDepth(p.images.depthEntityID, p.images.depthIndices);
 
        ARMARX_INFO << "Memory image structure: \n" << imageToArMem.summarizeStructure();
        ARMARX_VERBOSE << "Using image provider '" << p.imageProviderName << "'.";
 
        usingImageProvider(p.imageProviderName);
    }
 
    void
    visionx::ImageToArMem::onConnectImageProcessor()
    {
        // Connect to image provider.
        {
            getProxy(imageProvider, p.imageProviderName);
            imageProviderInfo = getImageProvider(p.imageProviderName);
 
            // Init input images.
            imageToArMem.initImages(imageProviderInfo.imageFormat);
            inputImages = imageToArMem.makeCByteImageBuffer();
            inputImageBuffer.clear();
            for (CByteImage& img : inputImages)
            {
                inputImageBuffer.emplace_back(&img);
            }
        }
 
        // Connect to memory server.
        {
            try
            {
                imageToArMem.setWriter(memoryNameSystem().useWriter(imageToArMem.getMemoryID()));
            }
            catch (const armarx::armem::error::CouldNotResolveMemoryServer& e)
            {
                ARMARX_ERROR << e.what();
            }
 
            imageToArMem.setDebugObserver(debugObserver);
 
            imageToArMem.addProviderSegments();
        }
 
        // Initially commit the camera calibraion
        if (p.commitCameraCalib)
        {
            if (visionx::StereoCalibrationProviderInterfacePrx calibrationProvider =
                    visionx::StereoCalibrationProviderInterfacePrx::checkedCast(
                        imageProviderInfo.proxy);
                calibrationProvider)
            {
                ARMARX_INFO
                    << "Found stereo camera calibration from StereoCalibrationProviderInterface.";
                visionx::StereoCalibration stereoCameraCalibration =
                    calibrationProvider->getStereoCalibration();
                imageToArMem.commitCameraCalibration(stereoCameraCalibration);
            }
            else if (visionx::StereoCalibrationInterfacePrx pointCloudAndStereoCalibrationProvider =
                         visionx::StereoCalibrationInterfacePrx::checkedCast(
                             imageProviderInfo.proxy);
                     calibrationProvider)
            {
                ARMARX_INFO << "Found stereo camera calibration from StereoCalibrationInterface.";
                visionx::StereoCalibration stereoCameraCalibration =
                    calibrationProvider->getStereoCalibration();
                imageToArMem.commitCameraCalibration(stereoCameraCalibration);
            }
            else
            {
                ARMARX_INFO << "Found monocular camera calibration. Could not cast to stereo "
                               "calibration interface.";
                visionx::MonocularCalibration monocularCameraCalibration =
                    visionx::tools::createDefaultMonocularCalibration();
 
                imageToArMem.commitCameraCalibration(monocularCameraCalibration);
            }
        }
    }
 
    void
    visionx::ImageToArMem::onDisconnectImageProcessor()
    {
    }
 
    void
    visionx::ImageToArMem::onExitImageProcessor()
    {
    }
 
    void
    visionx::ImageToArMem::process()
    {
        const armarx::Duration timeout = armarx::Duration::MilliSeconds(1000);
        armarx::Duration timeGetImages;
 
        int numImages = 0;
        if (waitForImages(p.imageProviderName, static_cast<int>(timeout.toMilliSeconds())))
        {
            TIMING_START(GetImages);
            numImages = getImages(p.imageProviderName, inputImageBuffer.data(), imageMetaInfo);
            TIMING_END_STREAM(GetImages, ARMARX_VERBOSE);
            timeGetImages = armarx::Duration::MicroSeconds(GetImages.toMicroSeconds());
 
            if (numImages == static_cast<int>(inputImageBuffer.size()))
            {
                using namespace armarx::armem;
                imageToArMem.useImageBuffers(
                    inputImageBuffer.data(),
                    Time(Duration::MicroSeconds(imageMetaInfo->timeProvided)));
                imageToArMem.commitImages();
            }
            else
            {
                ARMARX_ERROR << "Received unexpected number of input images. Got " << numImages
                             << " instead of " << inputImageBuffer.size() << ".";
            }
        }
        else
        {
            ARMARX_WARNING << "Timeout while waiting for camera images (> " << timeout << ").";
            return;
        }
 
        ARMARX_DEBUG << "Wait for next image";
 
        if (debugObserver)
        {
            debugObserver->setDebugChannel(
                getName(),
                {
                    {"getImages() [us]", new armarx::Variant(timeGetImages.toMicroSecondsDouble())},
                });
        }
    }
 
} // namespace visionx