TrackingError.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::TrackingError
 * @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 "TrackingError.h"
 
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
#include <ArmarXCore/observers/ObserverObjectFactories.h>
 
using namespace armarx;
 
armarx::PropertyDefinitionsPtr
TrackingError::createPropertyDefinitions()
{
    return armarx::PropertyDefinitionsPtr(
        new TrackingErrorPropertyDefinitions(getConfigIdentifier()));
}
 
void
armarx::TrackingError::onInitImageProcessor()
{
    providerName = getProperty<std::string>("providerName").getValue();
    usingImageProvider(providerName);
 
    offeringTopic(getProperty<std::string>("TrackingErrorTopicName").getValue());
 
    offeringTopic(getProperty<std::string>("DebugObserverName").getValue());
 
    frameRate = getProperty<float>("Framerate").getValue();
 
    useChessboard = getProperty<bool>("UseChessBoard").getValue();
 
    chessboardWidth = getProperty<int>("Chessboard.Width").getValue();
    chessboardHeight = getProperty<int>("Chessboard.Height").getValue();
}
 
void
armarx::TrackingError::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);
 
 
    debugObserver = getTopic<DebugObserverInterfacePrx>(
        getProperty<std::string>("DebugObserverName").getValue());
 
    prx = getTopic<TrackingErrorListenerPrx>(
        getProperty<std::string>("TrackingErrorTopicName").getValue());
 
    imageWidth = imageProviderPrx->getImageFormat().dimension.width;
    imageHeight = imageProviderPrx->getImageFormat().dimension.height;
 
    enableResultImages(
        1, imageProviderPrx->getImageFormat().dimension, imageProviderPrx->getImageFormat().type);
}
 
void
armarx::TrackingError::onExitImageProcessor()
{
}
 
void
armarx::TrackingError::process()
{
    std::unique_lock lock(imageMutex);
 
    if (!waitForImages(getProperty<std::string>("providerName").getValue(), 1000))
    {
        ARMARX_WARNING << "Timeout while waiting for camera images (>1000ms)";
        return;
    }
 
    int numImages = getImages(cameraImages);
 
    if (numImages == 0)
    {
        ARMARX_WARNING << "Didn't receive one image! Aborting!";
        return;
    }
 
    IplImage* ppIplImages[1] = {IplImageAdaptor::Adapt(cameraImages[0])};
 
    // convert to gray
    cv::Mat grayImage;
    cv::Mat resultImage = cv::cvarrToMat(ppIplImages[0]);
    cv::cvtColor(resultImage, grayImage, cv::COLOR_BGR2GRAY);
 
    std::vector<cv::Point2f> features;
 
    cv::Size size = cv::Size(chessboardWidth, chessboardHeight);
 
 
    if (useChessboard)
    {
        // + cv::CALIB_CB_FAST_CHECK)
        bool foundCorners = cv::findChessboardCorners(
            grayImage, size, features, cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_NORMALIZE_IMAGE);
 
        if (foundCorners)
        {
            float avgX = 0.f, avgY = 0.f;
            for (cv::Point2f point : features)
            {
                avgX += point.x;
                avgY += point.y;
            }
            avgX /= features.size();
            avgY /= features.size();
 
            cv::Point2f imgCenter(imageWidth / 2, imageHeight / 2);
            cv::Point2f chessboardCenter(avgX, avgY);
            cv::Point2f point(avgX, imageHeight / 2);
            cv::circle(resultImage, chessboardCenter, 2, CV_RGB(255, 0, 0), -1);
            cv::circle(resultImage, imgCenter, 2, CV_RGB(255, 0, 0), -1);
            cv::line(resultImage, chessboardCenter, imgCenter, CV_RGB(0, 0, 255), 1);
            cv::line(resultImage, chessboardCenter, point, CV_RGB(0, 255, 0), 1);
            cv::line(resultImage, imgCenter, point, CV_RGB(0, 255, 0), 1);
 
            float trackingErrorX = avgX - (imageWidth / 2);
            float trackingErrorY = avgY - (imageHeight / 2);
 
            float angleX = (trackingErrorX / imageWidth) * 59.7;
            float angleY = (trackingErrorY / imageHeight) * 44.775;
 
 
            StringVariantBaseMap debugValues;
            debugValues["trackingErrorX"] = new Variant(trackingErrorX);
            debugValues["trackingErrorY"] = new Variant(trackingErrorY);
            debugValues["angleX"] = new Variant(angleX);
            debugValues["angleY"] = new Variant(angleY);
 
 
            debugObserver->setDebugChannel("TrackingError", debugValues);
 
 
            prx->reportNewTrackingError(trackingErrorX, trackingErrorY, angleX, angleY);
        }
        else
        {
            ARMARX_WARNING << deactivateSpam(3) << "Chessboard NOT found!";
            return;
        }
    }
    else
    {
        //TODO: calculate tracking error with vectors
    }
 
 
    CByteImage* resultImages[1] = {IplImageAdaptor::Adapt(ppIplImages[0])};
    provideResultImages(resultImages);
 
    if (frameRate > 0.0)
    {
        fpsCounter.assureFPS(frameRate);
    }
}
 
namespace armarx
{
    ARMARX_REGISTER_COMPONENT_EXECUTABLE(::armarx::TrackingError, ::armarx::TrackingError::GetDefaultName());
} // namespace armarx