BigBowlLocalization.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     David Schiebener (schiebener at kit dot edu)
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "BigBowlLocalization.h"
 
// Core
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
 
// MemoryX
#include <MemoryX/core/entity/Entity.h>
#include <MemoryX/core/entity/ProbabilityMeasures.h>
#include <MemoryX/libraries/helpers/ObjectRecognitionHelpers/ObjectRecognitionWrapper.h>
 
// IVT
#include <Calibration/Calibration.h>
#include <Color/ColorParameterSet.h>
#include "Image/ByteImage.h"
#include "Image/ImageProcessor.h"
 
// IVTRecognition
#include "Locators/VisualTargetLocator.h"
 
namespace visionx
{
 
 
    BigBowlLocalization::BigBowlLocalization()
    {
        numberOfResultImages = 3;
        imageCounter = 1;
    }
 
 
    void BigBowlLocalization::onInitObjectLocalizerProcessor()
    {
        usingProxy("RobotStateComponent");
        useVision = getProperty<bool>("UseVision").getValue();
        objectColor = getProperty<ObjectColor>("BlobColor").getValue();
 
        // writing to the DebugDrawer topic. In ArmarX this topic's default name is DebugDrawerUpdates
        offeringTopic("DebugDrawerUpdates");
 
        stereoMatcher = new CStereoMatcher();
    }
 
    void BigBowlLocalization::onConnectObjectLocalizerProcessor()
    {
        robotStateComponent = getProxy<armarx::RobotStateComponentInterfacePrx>("RobotStateComponent");
        localRobot = armarx::RemoteRobot::createLocalClone(robotStateComponent->getSynchronizedRobot());
 
        // retrieve the proxy to the debug drawer
        debugDrawerPrx = getTopic<armarx::DebugDrawerInterfacePrx>("DebugDrawerUpdates");
 
        stereoMatcher->InitCameraParameters(getStereoCalibration(), false);
    }
 
    void BigBowlLocalization::onExitObjectLocalizerProcessor()
    {
        delete stereoMatcher;
    }
 
    bool BigBowlLocalization::initRecognizer()
    {
        if (useVision)
        {
            ARMARX_IMPORTANT <<   " xxxxxxxxxxx BigBowlLocalization initRecognizer!!!!!!!!!!" ;
 
            std::string colorParameterFilename = getProperty<std::string>("ColorParameterFile").getValue();
 
            if (!armarx::ArmarXDataPath::getAbsolutePath(colorParameterFilename, colorParameterFilename))
            {
                ARMARX_ERROR << "Could not find color parameter file in ArmarXDataPath: " << colorParameterFilename;
                return false;
            }
 
            ARMARX_VERBOSE << "Color parameter file name: " << colorParameterFilename;
 
            visualTargetLocator = new CVisualTargetLocator();
 
            if (!visualTargetLocator->Init(colorParameterFilename.c_str(), getStereoCalibration()))
            {
                return false;
            }
            int nMinPixelsPerRegion = getProperty<int>("nMinPixelsPerRegion").getValue();
            double dMaxEpipolarDiff = getProperty<double>("dMaxEpipolarDiff").getValue();
            double dMinFilledRatio = getProperty<double>("dMinFilledRatio").getValue();
            double dMaxSideRatio = getProperty<double>("dMaxSideRatio").getValue();
            double dMinSize = getProperty<double>("dMinSize").getValue();
            double dMaxSize = getProperty<double>("dMaxSize").getValue();
 
            visualTargetLocator->SetParameters(nMinPixelsPerRegion, dMaxEpipolarDiff,   dMinFilledRatio,  dMaxSideRatio, dMinSize,  dMaxSize);
 
            colorParameterSet = new CColorParameterSet();
            colorParameterSet->LoadFromFile(colorParameterFilename.c_str());
        }
        else
        {
            ARMARX_IMPORTANT << "Not using vision, will just return a fake result";
        }
 
        showSegmentedResultImages = getProperty<bool>("ShowSegmentedResultImages").getValue();
 
        if (!showSegmentedResultImages)
        {
            ::ImageProcessor::Zero(resultImages[2]);
        }
 
        return true;
    }
 
 
    memoryx::ObjectLocalizationResultList BigBowlLocalization::localizeObjectClasses(const std::vector<std::string>& objectClassNames, CByteImage** cameraImages, armarx::MetaInfoSizeBasePtr imageMetaInfo, CByteImage** resultImages)
    {
        ARMARX_CHECK_EXPRESSION(imageMetaInfo->timeProvided > 0);
        ARMARX_DEBUG << deactivateSpam(3) << "Image is already " << (IceUtil::Time::now() - IceUtil::Time::microSeconds(imageMetaInfo->timeProvided)).toMilliSecondsDouble() << "ms old";
        if (!armarx::RemoteRobot::synchronizeLocalCloneToTimestamp(localRobot, robotStateComponent, imageMetaInfo->timeProvided))
        {
            ARMARX_WARNING << deactivateSpam(1) << "Failed to synchronize local robot - results are probably wrong";
        }
 
        // check if reference frame is valid
        auto rnCamera = localRobot->getRobotNode(referenceFrameName);
        ARMARX_CHECK_EXPRESSION(rnCamera) << "Reference frame not present in remote robot!";
 
        const std::string agentName = localRobot->getName();
 
        Eigen::Vector3f expectedPositionVec;
        expectedPositionVec << 0, 600, 1000;
        const std::string rootFrameName = localRobot->getRootNode()->getName();
        armarx::FramedPositionPtr expectedPosition = new armarx::FramedPosition(expectedPositionVec, rootFrameName, agentName);
        expectedPosition->changeFrame(localRobot, referenceFrameName);
 
        memoryx::ObjectLocalizationResultList resultList;
 
        if (useVision)
        {
            // localize marker(s)
            Vec3d bigBowlPosition;
            int numFoundBlobs = 0;
 
            numFoundBlobs = locateBowl(cameraImages, bigBowlPosition, resultImages);
            ARMARX_INFO <<  "Found " << numFoundBlobs << " blobs.";
 
            if (numFoundBlobs == 0)
            {
                ARMARX_INFO << deactivateSpam(1) << "Object not found, color: " << objectColor;
            }
            else
            {
                // check if position is realistic
                Eigen::Vector3f position;
                position << bigBowlPosition.x, bigBowlPosition.y, bigBowlPosition.z;
                float diffToExpectedPosition = (position - expectedPosition->toEigen()).norm();
 
                if (diffToExpectedPosition < 1200)
                {
                    // assemble result
                    memoryx::ObjectLocalizationResult result;
                    result.recognitionCertainty = 0.96f;
                    result.objectClassName = objectClassNames.at(0);
 
                    armarx::FramedPositionPtr positionPtr = new armarx::FramedPosition(position, referenceFrameName, agentName);
                    positionPtr->changeFrame(localRobot, localRobot->getRootNode()->getName());
                    positionPtr->z  = 1030;
                    result.position = positionPtr;
                    Eigen::Matrix4f orientation = Eigen::Matrix4f::Identity();
                    VirtualRobot::MathTools::rpy2eigen4f(-M_PI / 2, 0, 0, orientation);
                    armarx::FramedOrientationPtr ori = new armarx::FramedOrientation(orientation, armarx::GlobalFrame, "");
                    ori->changeFrame(localRobot, referenceFrameName);
                    result.orientation = ori;
                    result.positionNoise = calculateLocalizationUncertainty(position);
                    result.timeStamp = new armarx::TimestampVariant(imageMetaInfo->timeProvided);
 
                    resultList.push_back(result);
 
                    ARMARX_VERBOSE << deactivateSpam(1) << "Object found at position " << position;
                    ARMARX_INFO <<  "position >> x: " <<  positionPtr->x << " y: " <<  positionPtr->y
                                << " z: " << positionPtr->z << " frame: " << positionPtr->frame;
                    armarx::FramedPositionPtr globalPosPtr = positionPtr->toGlobal(localRobot);
                    ARMARX_INFO << "globalposition >> x: " <<  globalPosPtr->x << " y: " <<  globalPosPtr->y
                                << " z: " << globalPosPtr->z << " frame: " << globalPosPtr->frame;
 
 
                    // setup a 4x4 homogeneous matrix
                    Eigen::Matrix4f currPose;
                    currPose.setIdentity();
                    // set rotating coordinates (mm)
                    currPose(0, 3) = globalPosPtr->x;
                    currPose(1, 3) = globalPosPtr->y;
                    currPose(2, 3) = globalPosPtr->z;
                    armarx::PosePtr poseGlobal(new armarx::Pose(currPose));
 
                    // size of the box in mm
                    Eigen::Vector3f v(100, 100, 100);
                    armarx::Vector3Ptr s(new armarx::Vector3(v));
                    // draw/update red box
                    debugDrawerPrx->clearLayer("objLocalizationLayer");
                    debugDrawerPrx->setBoxDebugLayerVisu("objLocalizationLayer", poseGlobal, s, armarx::DrawColor {1, 0, 0, 1});
 
                    // mark TCP pose with a cross
                    if (getResultImagesEnabled())
                    {
                        drawCrossInImage(resultImages[0], position, true);
                    }
                }
                else
                {
                    ARMARX_INFO << deactivateSpam(1) << "Object is too far away from expected position: " << (int)diffToExpectedPosition;
                }
            }
 
        }
        else
        {
            ARMARX_INFO << "Vision is switched off, returning fake result";
 
            memoryx::ObjectLocalizationResult result;
            result.recognitionCertainty = 0.8f;
            result.objectClassName = objectClassNames.at(0);
 
            result.position = expectedPosition;
 
            Eigen::Matrix3f orientation = Eigen::Matrix3f::Identity();
            result.orientation = new armarx::FramedOrientation(orientation, rootFrameName, agentName);
            result.positionNoise = calculateLocalizationUncertainty(expectedPosition->toEigen());
 
            resultList.push_back(result);
        }
 
        ARMARX_VERBOSE << "Finished localization, returning.";
 
        return resultList;
    }
 
 
 
    void BigBowlLocalization::drawCrossInImage(CByteImage* image, Eigen::Vector3f point, bool leftCamera)
    {
        Vec3d positionIVT = {point(0), point(1), point(2)};
        Vec2d projectedPoint;
 
        if (leftCamera)
        {
            getStereoCalibration()->GetLeftCalibration()->WorldToImageCoordinates(positionIVT, projectedPoint, getImagesAreUndistorted());
        }
        else
        {
            getStereoCalibration()->GetRightCalibration()->WorldToImageCoordinates(positionIVT, projectedPoint, getImagesAreUndistorted());
        }
 
        const int size = 15;
        ARMARX_INFO << "projectedPoint.x :" << projectedPoint.x << " projectedPoint.y: " << projectedPoint.y;
 
        if (size < projectedPoint.x && size < projectedPoint.y && projectedPoint.x < image->width - size - 1 && projectedPoint.y < image->height - size - 3)
        {
            for (int i = -size; i <= size; i++)
            {
                int indexHorizontalLine = 3 * ((int)projectedPoint.y * image->width + (int)projectedPoint.x + i);
                int indexVerticalLine = 3 * (((int)projectedPoint.y + i) * image->width + (int)projectedPoint.x);
                image->pixels[indexHorizontalLine] = 255;
                image->pixels[indexHorizontalLine + 1] = 255;
                image->pixels[indexHorizontalLine + 2] = 255;
                image->pixels[indexVerticalLine] = 255;
                image->pixels[indexVerticalLine + 1] = 255;
                image->pixels[indexVerticalLine + 2] = 255;
            }
        }
    }
 
    int BigBowlLocalization::locateBowl(CByteImage** cameraImages, Vec3d& bowlPosition, CByteImage** resultImages)
    {
        int numberOfSegments = 0;
        CByteImage* imageHSVLeft = new CByteImage(cameraImages[0]->width, cameraImages[0]->height, CByteImage::eRGB24);
        CByteImage* imageHSVRight = new CByteImage(cameraImages[1]->width, cameraImages[1]->height, CByteImage::eRGB24);
        CByteImage* imageFiltered = new CByteImage(cameraImages[0]->width, cameraImages[0]->height, CByteImage::eRGB24);
        CByteImage* imgLeftGray = new CByteImage(cameraImages[0]->width, cameraImages[1]->height, CByteImage::eGrayScale);
        CByteImage* imgRightGray = new CByteImage(cameraImages[1]->width, cameraImages[1]->height, CByteImage::eGrayScale);
 
        ::ImageProcessor::CalculateHSVImage(cameraImages[0], imageHSVLeft);
        ::ImageProcessor::CalculateHSVImage(cameraImages[1], imageHSVRight);
        ::ImageProcessor::ConvertImage(cameraImages[0], imgLeftGray);
        ::ImageProcessor::ConvertImage(cameraImages[1], imgRightGray);
        ::ImageProcessor::Zero(imageFiltered);
 
        int ThesholdHMinValue = 4;
        int ThesholdHMaxValue = 20;
        int ThesholdSMinValue = 220;
        int ThesholdSMaxValue = 255;
 
        int imagePosXLeft = 0;
        int imagePosYLeft = 0;
        int totalPixelLeft = 0;
        int imagePosXRight = 0;
        int imagePosYRight = 0;
        int totalPixelRight = 0;
        int deltaROI = 50;
 
        // scan the Left HSV image and compute the histogram
        for (int i = deltaROI; i < imageHSVLeft->width - deltaROI; i++)
        {
            for (int j = deltaROI; j < imageHSVLeft->height - deltaROI; j++)
            {
                int pixelIndex = (i + j * imageHSVLeft->width) * imageHSVLeft->bytesPerPixel;
                double hVal = (double) imageHSVLeft->pixels[pixelIndex];
                double sVal = (double) imageHSVLeft->pixels[pixelIndex + 1];
 
                if ((hVal > ThesholdHMinValue  &&  hVal < ThesholdHMaxValue) && (sVal > ThesholdSMinValue  &&  sVal < ThesholdSMaxValue))
                {
                    imageFiltered->pixels[pixelIndex] = cameraImages[0]->pixels[pixelIndex];
                    imageFiltered->pixels[pixelIndex + 1] = cameraImages[0]->pixels[pixelIndex + 1];
                    imageFiltered->pixels[pixelIndex + 2] = cameraImages[0]->pixels[pixelIndex + 2];
                    imagePosXLeft += i;
                    imagePosYLeft += j;
                    totalPixelLeft++;
                }
            }
        }
        ARMARX_INFO << "totalPixel >> left " << totalPixelLeft;
 
 
        // scan the Right HSV image and compute the histogram
        for (int i = deltaROI; i < imageHSVRight->width - deltaROI; i++)
        {
            for (int j = deltaROI; j < imageHSVRight->height - deltaROI; j++)
            {
                int pixelIndex = (i + j * imageHSVRight->width) * imageHSVRight->bytesPerPixel;
                double hVal = (double) imageHSVRight->pixels[pixelIndex];
                double sVal = (double) imageHSVRight->pixels[pixelIndex + 1];
 
                if ((hVal > ThesholdHMinValue  &&  hVal < ThesholdHMaxValue) && (sVal > ThesholdSMinValue  &&  sVal < ThesholdSMaxValue))
                {
                    imagePosXRight += i;
                    imagePosYRight += j;
                    totalPixelRight++;
                }
            }
        }
        ARMARX_INFO << "totalPixel >> right " << totalPixelRight;
 
        if (totalPixelLeft > 10000 && totalPixelRight > 10000)
        {
            numberOfSegments = 1;
        }
        else
        {
            resultImages[0] = cameraImages[0];
            resultImages[1] = cameraImages[1];
            ::ImageProcessor::Zero(imageFiltered);
            resultImages[2] = imageFiltered;
            numberOfSegments = 0;
            return numberOfSegments;
        }
 
        Vec2d imageCoorLeft, imageCoorRight;
        imageCoorLeft.x = (int) imagePosXLeft / totalPixelLeft;
        imageCoorLeft.y = (int) imagePosYLeft / totalPixelLeft;
        imageCoorRight.x = (int) imagePosXRight / totalPixelRight;
        imageCoorRight.y = (int) imagePosYRight / totalPixelRight;
 
        ARMARX_INFO << "imageCoorLeft x: " << imageCoorLeft.x <<  " y: " << imageCoorLeft.y;
        ARMARX_INFO << "imageCoorRight x: " << imageCoorRight.x <<  " y: " << imageCoorRight.y;
        // bowl must be somewhere in the middle in the left camera
        if (imageCoorLeft.x < 250 || imageCoorLeft.x > 470 || imageCoorLeft.y < 150 || imageCoorLeft.y > 320)
        {
            resultImages[0] = cameraImages[0];
            resultImages[1] = cameraImages[1];
            ::ImageProcessor::Zero(imageFiltered);
            resultImages[2] = imageFiltered;
            numberOfSegments = 0;
            return numberOfSegments;
        }
 
        Vec2d vCorrespondingPointRight;
        const int nDispMin = stereoMatcher->GetDisparityEstimate(10000);
        const int nDispMax = stereoMatcher->GetDisparityEstimate(500);
 
        // img l, img r, px, py, size of correlation window, min disparity, max disparity,
        // corresponding point 2d, 3d point, correlation threshold, images are undistorted
        int nMatchingResult = stereoMatcher->Match(imgLeftGray, imgRightGray, (int) imageCoorLeft.x, (int) imageCoorLeft.y,
                              50, nDispMin, nDispMax, vCorrespondingPointRight, bowlPosition, 0.7f, true);
 
 
        ARMARX_INFO << "nMatchingResult " << nMatchingResult;
        //bowlPosition.x -= 0.2;
        //ARMARX_INFO << "worldCoor x: " << bowlPosition.x <<  " y: " << bowlPosition.y << " z: " << bowlPosition.z;
 
        if (nMatchingResult <= 0)
        {
            resultImages[0] = cameraImages[0];
            resultImages[1] = cameraImages[1];
            ::ImageProcessor::Zero(imageFiltered);
            resultImages[2] = imageFiltered;
            numberOfSegments = 0;
            return numberOfSegments;
        }
 
        // draw a white dot in the object center
        int deltaCenter = 10;
        for (int i = imageCoorLeft.x - deltaCenter; i < imageCoorLeft.x + deltaCenter; i++)
        {
            for (int j = imageCoorLeft.y - deltaCenter; j < imageCoorLeft.y + deltaCenter; j++)
            {
                int pixelIndex = (i + j * imageHSVLeft->width) * imageHSVLeft->bytesPerPixel;
                imageFiltered->pixels[pixelIndex] = 255;
                imageFiltered->pixels[pixelIndex + 1] = 255;
                imageFiltered->pixels[pixelIndex + 2] = 255;
            }
        }
 
        /*
            std::stringstream str;
            str << "/common/homes/staff/aksoy/Desktop/testBowlImages/org_" << imageCounter++ << ".bmp";
            cameraImages[0]->SaveToFile(str.str().c_str());
            std::stringstream strFlt;
            strFlt << "/common/homes/staff/aksoy/Desktop/testBowlImages/flt_" << imageCounter++ << ".bmp";
            imageFiltered->SaveToFile(strFlt.str().c_str());
 
            ARMARX_INFO <<   " xxxxxxxxxxx Images are saved!!!!!!!!!!" << str.str().c_str();
        */
 
        resultImages[0] = cameraImages[0];
        resultImages[1] = cameraImages[1];
        resultImages[2] = imageFiltered;
        return numberOfSegments;
    }
}