OpenCVImageStabilizer.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::OpenCVImageStabilizer
 * @author     David Sippel ( uddoe at student dot kit dot edu )
 * @date       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "OpenCVImageStabilizer.h"
 
#include <VisionX/tools/OpenCVUtil.h>
 
#define SMOOTHING_RADIUS 3
 
using namespace armarx;
 
void
armarx::OpenCVImageStabilizer::onInitImageProcessor()
{
    providerName = getProperty<std::string>("providerName").getValue();
    usingImageProvider(providerName);
 
    frameRate = getProperty<float>("Framerate").getValue();
    drawFeatures = getProperty<bool>("drawFeatures").getValue();
}
 
void
armarx::OpenCVImageStabilizer::onConnectImageProcessor()
{
    std::unique_lock lock(imageMutex);
 
    visionx::ImageProviderInfo imageProviderInfo = getImageProvider(providerName);
    imageProviderPrx = getProxy<visionx::ImageProviderInterfacePrx>(providerName);
 
    cameraImages = new CByteImage*[2];
    cameraImages[0] = visionx::tools::createByteImage(imageProviderInfo);
    cameraImages[1] = visionx::tools::createByteImage(imageProviderInfo);
 
    //Only one result image (left camera image)
    enableResultImages(
        1, imageProviderPrx->getImageFormat().dimension, imageProviderPrx->getImageFormat().type);
}
 
void
armarx::OpenCVImageStabilizer::onExitImageProcessor()
{
    std::unique_lock lock(imageMutex);
    while (oldImages.size() != 0)
    {
        oldImages.front().release();
        oldImages.pop();
    }
    deltaTransformations.clear();
    prevFeatures.clear();
    prev.release();
    prevGrey.release();
    prevTransform.release();
}
 
void
armarx::OpenCVImageStabilizer::process()
{
    std::unique_lock lock(imageMutex);
 
    if (!waitForImages(getProperty<std::string>("providerName").getValue(), 1000))
    {
        ARMARX_WARNING << "Timeout while waiting for camera images (>1000ms)";
        return;
    }
 
    armarx::MetaInfoSizeBasePtr info;
    int numImages =
        getImages(getProperty<std::string>("providerName").getValue(), cameraImages, info);
 
    if (numImages == 0)
    {
        ARMARX_WARNING << "Didn't receive one image! Aborting!";
        return;
    }
    //Adapt only left camera image
    IplImage* ppIplImages[1] = {IplImageAdaptor::Adapt(cameraImages[0])};
 
    //convert to grey image
    cv::Mat currGrey;
    cv::Mat curr = cv::cvarrToMat(ppIplImages[0]);
    cv::cvtColor(curr, currGrey, cv::COLOR_BGR2GRAY);
 
    cv::Mat tmpImg(curr.size(), curr.type());
    curr.copyTo(tmpImg);
 
    //Parameters for goodFeaturesToTrack function
    int maxCorners = 100;
    double qualityLevel = 0.01;
    double minDistance = 5.0;
    cv::Mat mask;
    int blockSize = 3;
    bool useHarrisDetector = true;
    double k = 0.04;
    std::vector<cv::Point2f> currFeatures;
 
    cv::goodFeaturesToTrack(currGrey,
                            currFeatures,
                            maxCorners,
                            qualityLevel,
                            minDistance,
                            mask,
                            blockSize,
                            useHarrisDetector,
                            k);
 
    //Check if previos image is existing. If not, this is the first image, store it and return
    if (prev.data == NULL || prevGrey.data == NULL)
    {
        ARMARX_LOG << "First image";
        prev = curr;
        prevGrey = currGrey;
        prevFeatures = currFeatures;
        return;
    }
 
    std::vector<uchar> status;
    std::vector<float> error;
 
    cv::calcOpticalFlowPyrLK(prevGrey,
                             currGrey,
                             prevFeatures,
                             currFeatures,
                             status,
                             error,
                             cv::Size(21, 21),
                             cv::OPTFLOW_USE_INITIAL_FLOW);
 
    //Filter only overlapping features for transformation estimation
    std::vector<cv::Point2f> prevOverlapFeatures, currOverlapFeatures;
    for (unsigned int i = 0; i < status.size(); i++)
    {
        if (status[i] && error[i] < 25.f)
        {
            prevOverlapFeatures.push_back(prevFeatures[i]);
            currOverlapFeatures.push_back(currFeatures[i]);
            //ARMARX_LOG << deactivateSpam(1) << "error: " << error[i] << "    distance: " << std::sqrt(std::pow(prevFeatures[i].x - currFeatures[i].x, 2) + std::pow(prevFeatures[i].y - currFeatures[i].y, 2));
        }
    }
 
    cv::Mat currTransform =
        cv::estimateRigidTransform(prevOverlapFeatures, currOverlapFeatures, false);
 
    //If no transformation is found, use last valid transformation
    if (currTransform.data == NULL)
    {
        return;
        //prevTransform.copyTo(currTransform);
    }
 
    oldImages.push(tmpImg);
 
    currTransform.copyTo(prevTransform);
 
    //Get delta x, y and angle from transformation matrix
    double dx = currTransform.at<double>(0, 2);
    double dy = currTransform.at<double>(1, 2);
    double da = atan2(currTransform.at<double>(1, 0), currTransform.at<double>(0, 0));
 
    deltaTransformations.push_back(cv::Vec3d(dx, dy, da));
 
    //Check if there are already enough trajectory points to start smoothing
    if (deltaTransformations.size() <= 2 * SMOOTHING_RADIUS)
    {
        while (oldImages.size() > SMOOTHING_RADIUS + 2)
        {
            oldImages.pop();
        }
        return;
    }
 
    assert(oldImages.size() == SMOOTHING_RADIUS + 2);
 
    //index of currently processed frame
    int currFrameIndex = deltaTransformations.size() - SMOOTHING_RADIUS - 2;
 
    cv::Vec3d newTransformation(0, 0, 0);
 
    for (int i = -SMOOTHING_RADIUS; i <= SMOOTHING_RADIUS; i++)
    {
        newTransformation += -deltaTransformations[currFrameIndex + i];
    }
 
    newTransformation /= ((2 * SMOOTHING_RADIUS) + 1);
 
    //create transformation matrix
    cv::Mat transform(2, 3, CV_64F);
    transform.at<double>(0, 0) = cos(newTransformation[2]);
    transform.at<double>(0, 1) = -sin(newTransformation[2]);
    transform.at<double>(1, 0) = sin(newTransformation[2]);
    transform.at<double>(1, 1) = cos(newTransformation[2]);
 
    transform.at<double>(0, 2) = newTransformation[0];
    transform.at<double>(1, 2) = newTransformation[1];
 
    cv::Mat currImage = oldImages.front();
    oldImages.pop();
    cv::Mat result;
 
    //apply transformation to image
    cv::warpAffine(currImage, result, transform, currImage.size());
 
    if (drawFeatures)
    {
        for (const cv::Point2f& p : prevOverlapFeatures)
        {
            cv::circle(result, p, 2, CV_RGB(0, 255, 0), -1);
        }
    }
 
 
    CByteImage* output_cimage = new CByteImage();
    visionx::copyCvMatToIVT(result, output_cimage);
 
    CByteImage* resultImages[1] = {output_cimage};
    provideResultImages(std::move(resultImages));
}
 
armarx::PropertyDefinitionsPtr
OpenCVImageStabilizer::createPropertyDefinitions()
{
    return armarx::PropertyDefinitionsPtr(
        new OpenCVImageStabilizerPropertyDefinitions(getConfigIdentifier()));
}
 
void
armarx::OpenCVImageStabilizer::resetOpenCVStabilization(const Ice::Current&)
{
    std::unique_lock lock(imageMutex);
    ARMARX_LOG << "Resetting OpenCV stabilization!";
    while (oldImages.size() != 0)
    {
        oldImages.pop();
    }
    deltaTransformations.clear();
    prevFeatures.clear();
    prev.data = NULL;
    prevGrey.data = NULL;
    prevTransform.data = NULL;
}