ImageSequenceProvider.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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::Component
 * @author     Jan Issac (jan dot issac at gmx dot net)
 * @date       2011
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include <exception>
 
#include <Ice/Properties.h>
 
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/time/DateTime.h>
 
// VisionXCore
#include <VisionX/core/exceptions/user/StartingCaptureFailedException.h>
#include <VisionX/core/exceptions/user/FrameRateNotSupportedException.h>
 
// VisionXTools
#include <VisionX/tools/TypeMapping.h>
#include <VisionX/tools/ImageUtil.h>
#include <VisionX/tools/exceptions/local/InvalidFrameRateException.h>
 
#include "ImageSequenceProvider.h"
 
#include <Calibration/Calibration.h>
 
using namespace armarx;
 
namespace visionx
{
    void ImageSequenceProvider::onInitCapturingImageProvider()
    {
        // REQUIRED properties
        filePathLeft = getProperty<std::string>("PathLeft").getValue();
 
        // OPTIONAL properties
        filePathRight = getProperty<std::string>("PathRight").getValue();
 
        // image format
        setImageFormat(visionx::ImageDimension(getProperty<ImageDimension>("ImageSize").getValue()), visionx::eRgb);
        setImageSyncMode(eFpsSynchronization);
        frameRate = getProperty<float>("FrameRate").getValue();
 
        // open the image files
        bitmapSequenceCapture = NULL;
        images = NULL;
        OpenImageFiles();
        loadNewImageAtNextCapture = true;
 
        // stereo calibration
        std::string calibrationFileName = getProperty<std::string>("CalibrationFile").getValue();
        if (!calibrationFileName.empty())
        {
            std::string fullCalibrationFileName;
 
            if (!ArmarXDataPath::getAbsolutePath(calibrationFileName, fullCalibrationFileName))
            {
                ARMARX_ERROR << "Could not find camera calibration file in ArmarXDataPath: " << calibrationFileName;
            }
 
            if (!ivtStereoCalibration.LoadCameraParameters(
                    fullCalibrationFileName.c_str(), true))
            {
                ARMARX_ERROR << "Error loading camera calibration file: " << fullCalibrationFileName;
            }
        }
 
        stereoCalibration = visionx::tools::convert(ivtStereoCalibration);
 
        repeatImageCount = getProperty<int>("RepeatImageCount");
    }
 
 
    void ImageSequenceProvider::onExitCapturingImageProvider()
    {
        if (images != NULL)
        {
            for (int i = 0; i < getNumberImages(); i++)
            {
                delete images[i];
                delete resizedImages[i];
            }
 
            delete [] images;
            delete [] resizedImages;
 
            images = NULL;
        }
    }
 
 
    void ImageSequenceProvider::onStartCapture(float frameRate)
    {
        if (!bitmapSequenceCapture->OpenCamera())
        {
            throw visionx::exceptions::user::StartingCaptureFailedException(
                "Opening bitmap sequence \"" + filePathLeft + "\" failed.");
        }
        else
        {
            loadNewImageAtNextCapture = true;
            currentRepeatCount = repeatImageCount - 1;
        }
    }
 
 
    void ImageSequenceProvider::onStopCapture()
    {
        bitmapSequenceCapture->CloseCamera();
    }
 
 
    bool ImageSequenceProvider::capture(void** ppImageBuffers)
    {
        bool succeeded = true;
 
        if (loadNewImageAtNextCapture)
        {
            std::unique_lock lock(captureMutex);
 
            ARMARX_VERBOSE << "Loading next image";
 
            loadNewImageAtNextCapture = getProperty<bool>("LoadNextImageAutomatically").getValue();
 
            ++currentRepeatCount;
            if (currentRepeatCount == repeatImageCount)
            {
                succeeded = bitmapSequenceCapture->CaptureImage(images);
                currentRepeatCount = 0;
 
                if (!succeeded)
                {
                    bitmapSequenceCapture->Rewind();
                    succeeded = bitmapSequenceCapture->CaptureImage(images);
                }
            }
        }
 
        if (succeeded)
        {
            armarx::SharedMemoryScopedWriteLockPtr lock = getScopedWriteLock();
 
            for (int i = 0; i < getNumberImages(); i++)
            {
                ::ImageProcessor::Resize(images[i], resizedImages[i]);
 
                memcpy(ppImageBuffers[i], resizedImages[i]->pixels,
                       getImageFormat().dimension.width * getImageFormat().dimension.height * getImageFormat().bytesPerPixel);
            }
        }
 
        updateTimestamp(DateTime::Now());
 
        return succeeded;
    }
 
 
    StereoCalibration ImageSequenceProvider::getStereoCalibration(const Ice::Current& c)
    {
        return stereoCalibration;
    }
 
 
    bool ImageSequenceProvider::getImagesAreUndistorted(const Ice::Current& c)
    {
        return getProperty<bool>("ImagesAreUndistorted").getValue();
    }
 
 
    void ImageSequenceProvider::setImageFilePath(const std::string& imageFilePathLeft, const std::string& imageFilePathRight, const Ice::Current& c)
    {
        filePathLeft = imageFilePathLeft;
        filePathRight = imageFilePathRight;
 
        OpenImageFiles();
    }
 
 
    void ImageSequenceProvider::OpenImageFiles()
    {
        std::unique_lock lock(captureMutex);
 
        bool isStereoMode = filePathRight.length() > 0;
 
        std::string fullFilePathLeft, fullFilePathRight;
 
        if (!ArmarXDataPath::getAbsolutePath(filePathLeft, fullFilePathLeft))
        {
            ARMARX_ERROR << "Could not find left image sequence file in ArmarXDataPath: " << filePathLeft;
        }
 
        if (isStereoMode && !ArmarXDataPath::getAbsolutePath(filePathRight, fullFilePathRight))
        {
            ARMARX_ERROR << "Could not find right image sequence file in ArmarXDataPath: " << filePathRight;
        }
 
        if (bitmapSequenceCapture)
        {
            delete bitmapSequenceCapture;
        }
 
        bitmapSequenceCapture = new CBitmapSequenceCapture(fullFilePathLeft.c_str(), (isStereoMode ? fullFilePathRight.c_str() : NULL));
 
        // create images
        int width = 0;
        int height = 0;
 
        if (bitmapSequenceCapture->OpenCamera())
        {
            width = bitmapSequenceCapture->GetWidth();
            height = bitmapSequenceCapture->GetHeight();
 
            loadNewImageAtNextCapture = true;
            //bitmapSequenceCapture->CloseCamera();
        }
        else
        {
            ARMARX_ERROR << "Could not open bitmap sequence";
        }
 
        if (images)
        {
            for (int i = 0; i < getNumberImages(); i++)
            {
                delete images[i];
                delete resizedImages[i];
            }
 
            delete [] images;
            delete [] resizedImages;
        }
 
 
        setNumberImages((isStereoMode ? 2 : 1));
 
 
        images = new CByteImage*[getNumberImages()];
 
        for (int i = 0; i < getNumberImages(); i++)
        {
            images[i] = new CByteImage(width, height, CByteImage::eRGB24);
        }
 
        resizedImages = new CByteImage*[getNumberImages()];
 
        for (int i = 0 ; i < getNumberImages() ; i++)
        {
            resizedImages[i] = visionx::tools::createByteImage(getImageFormat(), getImageFormat().type);
        }
    }
 
 
    void ImageSequenceProvider::loadNextImage(const Ice::Current& c)
    {
        std::unique_lock lock(captureMutex);
 
        loadNewImageAtNextCapture = true;
    }
 
}