FlyCaptureImageProvider.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::ArmarXObjects::FlyCaptureImageProvider
 * @author     Markus Grotz ( markus dot grotz at kit dot edu )
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "FlyCaptureImageProvider.h"
 
 
#include <VisionX/tools/TypeMapping.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
 
#include <Calibration/StereoCalibration.h>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
 
using namespace armarx;
 
 
void armarx::FlyCaptureImageProvider::onInitCapturingImageProvider()
{
    std::string value = getProperty<std::string>("serialNumbers").getValue();
    std::vector<std::string> serialNumbers = simox::alg::split(value, "\t ,");
 
    visionx::ImageDimension dimensions = getProperty<visionx::ImageDimension>("dimensions").getValue();
 
    unsigned int numCameras;
    if (busManager.GetNumOfCameras(&numCameras) != FlyCapture2::PGRERROR_OK && !numCameras)
    {
        ARMARX_FATAL << "no cameras found or unable to query bus";
        return;
    }
 
    ARMARX_INFO << "found " << numCameras << " cameras";
 
    for (std::string& serialNumber : serialNumbers)
    {
        FlyCapture2::PGRGuid pGuid;
 
        if (busManager.GetCameraFromSerialNumber(std::stoi(serialNumber), &pGuid) != FlyCapture2::PGRERROR_OK)
        {
            ARMARX_FATAL << "invalid serial number or camera not found: " << serialNumber;
        }
 
        FlyCapture2::Camera* c = new FlyCapture2::Camera();
 
 
 
        if (c->Connect(&pGuid) != FlyCapture2::PGRERROR_OK)
        {
            ARMARX_FATAL << "unable to connect to camera!";
        }
 
        FlyCapture2::CameraInfo cameraInfo;
        if (c->GetCameraInfo(&cameraInfo) != FlyCapture2::PGRERROR_OK)
        {
            ARMARX_WARNING << "unable to get camera info";
        }
        else
        {
            ARMARX_INFO <<  " camera model: " << cameraInfo.modelName << " serial number: `" << cameraInfo.serialNumber;
        }
 
        cameras.push_back(c);
 
 
        colorImages.push_back(new FlyCapture2::Image(dimensions.height, dimensions.width, FlyCapture2::PIXEL_FORMAT_RGB, FlyCapture2::BayerTileFormat::RGGB));
    }
 
 
    cameraImages = new CByteImage*[cameras.size()];
    for (size_t i = 0 ; i < cameras.size(); i++)
    {
        cameraImages[i] = new CByteImage(dimensions.width, dimensions.height, CByteImage::eRGB24);
    }
 
    rectifyImages = false;
 
    undistortion = new CUndistortion();
    undistortImages = getProperty<bool>("UndistortImages").getValue();
    undistortion->Init(calibrationFileName.c_str());
 
    frameRate = getProperty<float>("FrameRate").getValue();
 
    setNumberImages(cameras.size());
    setImageFormat(dimensions, visionx::eRgb, visionx::eBayerPatternRg);
    setImageSyncMode(visionx::eCaptureSynchronization);
 
 
}
 
void armarx::FlyCaptureImageProvider::onExitCapturingImageProvider()
{
    for (FlyCapture2::Camera* c : cameras)
    {
        if (c->IsConnected())
        {
            c->Disconnect();
        }
 
        delete c;
    }
 
    for (FlyCapture2::Image* i : colorImages)
    {
        delete i;
    }
 
 
    for (size_t i = 0; i < cameras.size(); i++)
    {
        delete cameraImages[i];
    }
 
    delete [] cameraImages;
 
    delete undistortion;
}
 
void armarx::FlyCaptureImageProvider::onStartCapture(float framesPerSecond)
{
    for (FlyCapture2::Camera* c : cameras)
    {
        FlyCapture2::Property p;
        p.absControl = true;
        p.onOff = true;
 
        p.type = FlyCapture2::AUTO_EXPOSURE;
        p.absValue = getProperty<float>("Exposure").getValue();
        p.autoManualMode = getProperty<float>("Exposure").getValue() >= 0;
        if (c->SetProperty(&p) != FlyCapture2::PGRERROR_OK)
        {
            ARMARX_WARNING << "unable to set auto exposure property";
        }
 
        p.type = FlyCapture2::SHUTTER;
        p.absValue = getProperty<float>("Shutter").getValue();
        p.autoManualMode = getProperty<float>("Shutter").getValue() >= 0;
        if (c->SetProperty(&p) != FlyCapture2::PGRERROR_OK)
        {
            ARMARX_WARNING << "unable to set shutter property";
        }
 
        p.type = FlyCapture2::GAIN;
        p.absValue = getProperty<float>("Gain").getValue();
        p.autoManualMode = getProperty<float>("Gain").getValue() >= 0;
        if (c->SetProperty(&p) != FlyCapture2::PGRERROR_OK)
        {
            ARMARX_WARNING << "unable to set gain property";
        }
 
 
        FlyCapture2::FrameRate frameRate = FlyCapture2::FRAMERATE_7_5;
 
        if (framesPerSecond >= 60.0)
        {
            frameRate = FlyCapture2::FRAMERATE_60;
        }
        else if (framesPerSecond >= 30.0)
        {
            frameRate = FlyCapture2::FRAMERATE_30;
        }
        else if (framesPerSecond >= 15.0)
        {
            frameRate = FlyCapture2::FRAMERATE_15;
        }
        else if (framesPerSecond >= 7.5)
        {
            frameRate = FlyCapture2::FRAMERATE_7_5;
        }
        else
        {
            ARMARX_WARNING << "unsupported frame rate";
        }
 
        FlyCapture2::VideoMode mode = FlyCapture2::VideoMode::VIDEOMODE_640x480Y8;
 
 
        if (getProperty<visionx::ImageDimension>("dimensions").getValue() == visionx::ImageDimension(640, 480))
        {
            mode = FlyCapture2::VideoMode::VIDEOMODE_640x480Y8;
        }
        else if (getProperty<visionx::ImageDimension>("dimensions").getValue() == visionx::ImageDimension(1600, 1200))
        {
            mode = FlyCapture2::VideoMode::VIDEOMODE_1600x1200Y8;
        }
        else
        {
            ARMARX_WARNING << "unsupported video mode";
        }
 
        FlyCapture2::Error error = c->SetVideoModeAndFrameRate(mode, frameRate);
 
        if (error != FlyCapture2::PGRERROR_OK)
        {
            ARMARX_WARNING << "unable to set video mode";
        }
 
        if (c->StartCapture() != FlyCapture2::PGRERROR_OK)
        {
            ARMARX_WARNING << "unable to start capture!";
        }
    }
}
 
void armarx::FlyCaptureImageProvider::onStopCapture()
{
    for (FlyCapture2::Camera* c : cameras)
    {
        c->StopCapture();
    }
}
 
bool FlyCaptureImageProvider::capture(void** ppImageBuffers)
{
    visionx::ImageFormatInfo imageFormat = getImageFormat();
    const int imageSize = imageFormat.dimension.width * imageFormat.dimension.height * imageFormat.bytesPerPixel;
 
    FlyCapture2::Image imageBuffer;
    for (size_t i = 0; i < cameras.size(); i++)
    {
 
        if (cameras[i]->RetrieveBuffer(&imageBuffer) != FlyCapture2::PGRERROR_OK)
        {
            ARMARX_WARNING << "failed to get camera buffer";
            continue;
            //return false;
        }
 
        if (imageBuffer.Convert(FlyCapture2::PIXEL_FORMAT_RGB, colorImages[i]) != FlyCapture2::PGRERROR_OK)
        {
            ARMARX_WARNING << "failed to convert image";
            return false;
        }
    }
 
    armarx::SharedMemoryScopedWriteLockPtr lock(sharedMemoryProvider->getScopedWriteLock());
 
    for (size_t i = 0; i < cameras.size(); i++)
    {
        memcpy(cameraImages[i]->pixels, colorImages[i]->GetData(), imageSize);
    }
 
    if (rectifyImages)
    {
        rectification->Rectify(cameraImages, cameraImages);
    }
    else if (undistortImages)
    {
        undistortion->Undistort(cameraImages, cameraImages);
    }
 
    for (size_t i = 0; i < cameras.size(); i++)
    {
        memcpy(ppImageBuffers[i], cameraImages[i]->pixels, imageSize);
    }
 
 
    return true;
}
 
PropertyDefinitionsPtr FlyCaptureImageProvider::createPropertyDefinitions()
{
    return PropertyDefinitionsPtr(new FlyCaptureImageProviderPropertyDefinitions(
                                      getConfigIdentifier()));
}
 
 
void armarx::FlyCaptureStereoCameraProvider::onInitCapturingImageProvider()
{
    FlyCaptureImageProvider::onInitCapturingImageProvider();
 
    calibrationFileName = getProperty<std::string>("CalibrationFile") .getValue();
    if (!armarx::ArmarXDataPath::getAbsolutePath(calibrationFileName, calibrationFileName))
    {
        ARMARX_ERROR << "" << calibrationFileName.c_str();
    }
 
    CStereoCalibration ivtStereoCalibration;
    if (!ivtStereoCalibration.LoadCameraParameters(calibrationFileName.c_str(), true))
    {
        ARMARX_ERROR << "" << calibrationFileName.c_str();
    }
 
    rectifyImages = getProperty<bool>("RectifyImages").getValue();
    rectification = new CRectification(true, undistortImages);
    rectification->Init(calibrationFileName.c_str());
    stereoCalibration = visionx::tools::convert(ivtStereoCalibration);
}