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"
 
#include <VirtualRobot/MathTools.h>
 
// Core
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
 
#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
// MemoryX
#include <MemoryX/core/entity/Entity.h>
#include <MemoryX/core/entity/ProbabilityMeasures.h>
#include <MemoryX/libraries/helpers/ObjectRecognitionHelpers/ObjectRecognitionWrapper.h>
 
// IVT
#include "Image/ByteImage.h"
#include "Image/ImageProcessor.h"
#include <Calibration/Calibration.h>
#include <Color/ColorParameterSet.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;
    }
} // namespace visionx