OpticalFlow.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::OpticalFlow
 * @author     David Sippel ( uddoe at student dot kit dot edu )
 * @date       2016
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "OpticalFlow.h"
 
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
#include <ArmarXCore/observers/ObserverObjectFactories.h>
 
using namespace armarx;
 
armarx::PropertyDefinitionsPtr
OpticalFlow::createPropertyDefinitions()
{
    return armarx::PropertyDefinitionsPtr(
        new OpticalFlowPropertyDefinitions(getConfigIdentifier()));
}
 
void
armarx::OpticalFlow::onInitImageProcessor()
{
    providerName = getProperty<std::string>("providerName").getValue();
    usingImageProvider(providerName);
 
    offeringTopic(getProperty<std::string>("OpticalFlowTopicName").getValue());
    offeringTopic(getProperty<std::string>("DebugObserverName").getValue());
 
    frameRate = getProperty<float>("Framerate").getValue();
 
    method = getProperty<OPTICAL_FLOW_METHOD>("Method").getValue();
 
    chessboardWidth = getProperty<int>("Chessboard.Width").getValue();
    chessboardHeight = getProperty<int>("Chessboard.Height").getValue();
 
    usingTopic(getProperty<std::string>("CalibrationUpdateTopicName").getValue());
}
 
void
armarx::OpticalFlow::onConnectImageProcessor()
{
    debugObserver = getTopic<DebugObserverInterfacePrx>(
        getProperty<std::string>("DebugObserverName").getValue());
 
 
    visionx::ImageProviderInfo imageProviderInfo = getImageProvider(providerName);
    imageDimension = imageProviderInfo.imageFormat.dimension;
 
    imageProviderPrx = getProxy<visionx::ImageProviderInterfacePrx>(providerName);
 
    cameraImages = new CByteImage*[2];
    cameraImages[0] = visionx::tools::createByteImage(imageProviderInfo);
    cameraImages[1] = visionx::tools::createByteImage(imageProviderInfo);
 
 
    prx = getTopic<OpticalFlowListenerPrx>(
        getProperty<std::string>("OpticalFlowTopicName").getValue());
 
    enableResultImages(1, imageDimension, visionx::ImageType::eRgb);
 
 
    visionx::StereoCalibrationProviderInterfacePrx calibrationProvider =
        visionx::StereoCalibrationProviderInterfacePrx::checkedCast(imageProviderPrx);
 
    if (calibrationProvider)
    {
        reportStereoCalibrationChanged(calibrationProvider->getStereoCalibration(), false, "");
    }
    else
    {
        ARMARX_WARNING << "unable to obtain calibration parameters. using default values";
 
        unitFactorX = (1.0 / 640.0) * (59.7);
        unitFactorY = (1.0 / 480.0) * (44.775);
    }
}
 
void
armarx::OpticalFlow::onExitImageProcessor()
{
}
 
void
armarx::OpticalFlow::process()
{
    /******************
    * Get gray image
    ******************/
 
    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;
    }
 
    IplImage* ppIplImages[1] = {IplImageAdaptor::Adapt(cameraImages[1])};
 
    // convert to gray
    cv::Mat grayImage;
    cv::Mat resultImage = cv::cvarrToMat(ppIplImages[0]);
    cv::cvtColor(resultImage, grayImage, cv::COLOR_BGR2GRAY);
 
    /******************
    * Compute optical flow
    ******************/
 
    std::vector<cv::Point2f> features;
 
    timeDiff = (info->timeProvided - previousTime) / 1000000.0;
 
    if ((timeDiff > 1.0) || !previousFeatures.size())
    {
        if (timeDiff > 1.0)
        {
            ARMARX_INFO << "time delta is larger than 1 sec. discarding previous features.";
        }
 
        if (!previousFeatures.size())
        {
            ARMARX_INFO << "no feature found on previous image. discarding previous image.";
        }
 
        resultX = 0.;
        resultY = 0.;
    }
    else
    {
        ComputeOpticalFlow(resultImage, grayImage);
 
 
        // ARMARX_LOG << deactivateSpam(5) << "avgDiffX: " << resultX << "      avgDiffY: " << resultY << " time " << timeDiff;
    }
 
 
    prx->reportNewOpticalFlow(resultX, resultY, timeDiff, previousTime);
 
    StringVariantBaseMap debugValues;
    debugValues["x"] = new Variant(resultX);
    debugValues["y"] = new Variant(resultY);
    debugValues["delta_t"] = new Variant((timeDiff));
 
 
    debugObserver->setDebugChannel(getName(), debugValues);
 
 
    /******************
    * Get Next Features
    ******************/
 
    if (method == Chessboard)
    {
        cv::Size size = cv::Size(chessboardWidth, chessboardHeight);
 
        // + cv::CALIB_CB_FAST_CHECK)
        bool foundCorners = cv::findChessboardCorners(
            grayImage, size, features, cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_NORMALIZE_IMAGE);
 
        cv::drawChessboardCorners(resultImage, size, features, foundCorners);
        if (!foundCorners)
        {
            ARMARX_WARNING << "Chessboard NOT found!";
        }
    }
 
    else if (method == Feature)
    {
        int maxCorners = 100;
        double qualityLevel = 0.01;
        double minDistance = 10.0;
        cv::Mat mask1;
        int blockSize = 3;
        bool useHarrisDetector = true;
        double k = 0.04;
 
        cv::goodFeaturesToTrack(grayImage,
                                features,
                                maxCorners,
                                qualityLevel,
                                minDistance,
                                mask1,
                                blockSize,
                                useHarrisDetector,
                                k);
    }
 
    if (method == Dense)
    {
        cv::Mat flow = cv::Mat(grayImage.rows, grayImage.cols, CV_32FC1);
        double mean_flow = 0.;
        double rmse_flow = 0.;
        if (method == Dense)
        {
            if (previousImage.size() == grayImage.size())
            {
                cv::calcOpticalFlowFarneback(
                    previousImage, grayImage, flow, 0.5, 10, 45, 3, 9, 1.2, 0);
                flow = (59.7 / 640.) * flow; // pixel to deg
                flow = (1.0 / timeDiff) * flow; // per frame to per s
                computeAverageDenseFlow(flow, mean_flow, rmse_flow);
            }
        }
 
        // drawn optical flow
        drawDenseFlowMap(flow, resultImage, 10, CV_RGB(0, 255, 0));
 
        // send values for plotting
        StringVariantBaseMap debugValues;
        debugValues["mean_dense"] = new Variant((mean_flow));
        debugValues["rmse_dense"] = new Variant((rmse_flow));
        debugObserver->setDebugChannel(getName(), debugValues);
    }
 
 
    CByteImage* resultImages[1] = {IplImageAdaptor::Adapt(ppIplImages[0])};
    provideResultImages(resultImages);
 
    if (method != Dense)
    {
        if (!features.size())
        {
            ARMARX_WARNING << "no features found";
            //return;
        }
    }
 
    previousImage = grayImage;
    previousFeatures = features;
    previousTime = info->timeProvided;
 
    lock.unlock();
 
    if (frameRate > 0.0)
    {
        fpsCounter.assureFPS(frameRate);
    }
}
 
void
OpticalFlow::ComputeOpticalFlow(cv::Mat resultImage, cv::Mat grayImage)
{
    std::vector<uchar> status;
    std::vector<float> error;
 
    std::vector<cv::Point2f> trackedFeatures;
    cv::calcOpticalFlowPyrLK(previousImage,
                             grayImage,
                             previousFeatures,
                             trackedFeatures,
                             status,
                             error,
                             cv::Size(21, 21),
                             cv::OPTFLOW_USE_INITIAL_FLOW);
 
    std::vector<float> avgDiffx, avgDiffy;
    for (size_t i = 0; i < trackedFeatures.size(); i++)
    {
        if (status[i])
        {
            avgDiffx.push_back(previousFeatures[i].x - trackedFeatures[i].x);
            avgDiffy.push_back(previousFeatures[i].y - trackedFeatures[i].y);
 
            cv::circle(resultImage, previousFeatures[i], 2, CV_RGB(255, 0, 0), -1);
            cv::circle(resultImage, trackedFeatures[i], 2, CV_RGB(0, 0, 255), -1);
            cv::line(resultImage, previousFeatures[i], trackedFeatures[i], CV_RGB(255, 0, 0), 1);
        }
    }
 
    if (method == Feature)
    {
        //ARMARX_LOG << deactivateSpam(1) << "Removing " << (avgDiffx.size() / 20) << " from x with size: " << avgDiffx.size();
        //ARMARX_LOG << deactivateSpam(1) << "Removing " << (avgDiffy.size() / 20) << " from y with size: " << avgDiffy.size();
 
        for (size_t i = 0; i < (avgDiffx.size() / 20); i++)
        {
            avgDiffx = removeHighestAndLowestMember(avgDiffx);
        }
        for (size_t i = 0; i < (avgDiffy.size() / 20); i++)
        {
            avgDiffy = removeHighestAndLowestMember(avgDiffy);
        }
    }
 
    if (!avgDiffx.size() || !avgDiffy.size())
    {
        ARMARX_WARNING << "No features found in the image, return = optical Flow!";
        resultX = 0.;
        resultY = 0.;
        return;
    }
 
    Eigen::Map<Eigen::ArrayXf> diffX(avgDiffx.data(), avgDiffx.size());
    Eigen::Map<Eigen::ArrayXf> diffY(avgDiffy.data(), avgDiffy.size());
 
    // convert to rad/sec
    resultX = diffX.mean() / timeDiff;
    resultY = diffY.mean() / timeDiff;
 
    resultX *= unitFactorX;
    resultY *= unitFactorY;
}
 
std::vector<float>
OpticalFlow::removeHighestAndLowestMember(std::vector<float>& input)
{
    std::sort(input.begin(), input.end());
    return std::vector<float>(&input[1], &input[input.size() - 1]);
}
 
void
OpticalFlow::drawDenseFlowMap(const cv::Mat& flow,
                              cv::Mat& cflowmap,
                              int step,
                              const cv::Scalar& color)
{
 
    for (int y = 0; y < cflowmap.rows; y += step)
        for (int x = 0; x < cflowmap.cols; x += step)
        {
            const cv::Point2f& fxy = flow.at<cv::Point2f>(y, x);
            cv::line(cflowmap,
                     cv::Point(x, y),
                     cv::Point(cvRound(x + fxy.x), cvRound(y + fxy.y)),
                     color);
            cv::circle(cflowmap, cv::Point(cvRound(x + fxy.x), cvRound(y + fxy.y)), 1, color, -1);
        }
}
 
// average optical flow norm on a window of W/2 x H/2 centered in the image
void
OpticalFlow::computeAverageDenseFlow(const cv::Mat& flow, double& mean, double& rmse)
{
 
    double dist = 0.;
    double dist2 = 0.;
    double d;
    double count = 0.;
 
    for (int y = round(flow.rows / 4); y < round(flow.rows * 3 / 4); y++)
    {
        for (int x = round(flow.cols / 4); x < round(flow.cols * 3 / 4); x++)
        {
            const cv::Point2f& fxy = flow.at<cv::Point2f>(y, x);
            d = fxy.x * fxy.x + fxy.y * fxy.y;
            dist += sqrt(d);
            dist2 += d;
            count++;
        }
    }
 
    rmse = sqrt(dist2 / count);
    mean = dist / count;
}
 
namespace armarx
{
    ARMARX_REGISTER_COMPONENT_EXECUTABLE(::armarx::OpticalFlow, ::armarx::OpticalFlow::GetDefaultName());
} // namespace armarx