ObjectRecognition.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
 * @author
 * @date
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#include "ObjectRecognition.h"
 
#include "OLPTools.h"
#include "ObjectLearningByPushingDefinitions.h"
#include "PointCloudRegistration.h"
 
// IVT
#include <Calibration/Calibration.h>
#include <Calibration/StereoCalibration.h>
#include <Image/ByteImage.h>
#include <Image/ImageProcessor.h>
 
#ifdef OLP_USE_EARLYVISION
// EV
#include </home/staff/schieben/EarlyVision/src/Base/Features/RFCH/Descriptors/HueDescriptor.h>
#include </home/staff/schieben/EarlyVision/src/Base/Features/RFCH/Descriptors/HueLaplacianDescriptor.h>
#include </home/staff/schieben/EarlyVision/src/Base/Features/RFCH/RFCSignatureCalculator.h>
#include </home/staff/schieben/EarlyVision/src/Base/Features/RFCH/RFCSignatureFeatureEntry.h>
#include </home/staff/schieben/EarlyVision/src/Base/Vision/Recognition/AdditiveRegionImage.h>
#endif
 
// OpenMP
#include <omp.h>
 
// stdlib
#include <cmath>
#include <cstdlib>
 
#include <ArmarXCore/core/logging/Logging.h>
 
// #include <fstream>
#include <pcl/io/pcd_io.h>
 
void
CObjectRecognition::SaveObjectDescriptorPCD(const CObjectHypothesis* pHypothesis,
                                            const std::string sObjectName,
                                            const int nDescriptorNumber)
{
    std::string sFileName = OLP_OBJECT_LEARNING_DIR;
    sFileName.append(sObjectName);
    ARMARX_VERBOSE_S << "Saving object descriptor. File name: " << sFileName.c_str();
 
    // save the confirmed points
    std::string sDescNum = std::to_string(nDescriptorNumber);
    sFileName += "-cloud" + std::string(2 - sDescNum.length(), '0') + sDescNum + ".pcd";
 
    auto cloud = COLPTools::ConfirmedPointsToPCL(pHypothesis);
    // pcl::io::savePCDFileASCII(sFileName, *cloud);
    pcl::io::savePCDFileBinary(sFileName, *cloud);
 
    /*
    std::ofstream fs(sFileName);
    if (!fs.is_open())
    {
        ARMARX_WARNING << "Could not write to file '" << sFileName << ".pcd'" << armarx::flush;
        return;
    }
 
    fs << "VERSION 0.7\n";
    fs << "FIELDS x y z rgba\n";
    fs << "SIZE 4 4 4 4\n";
    fs << "TYPE F F F U\n";
    fs << "COUNT 1 1 1 1\n";
    fs << "WIDTH " << pHypothesis->aConfirmedPoints.size() << "\n";
    fs << "HEIGHT 1\n";
    fs << "VIEWPOINT 0 0 0 1 0 0 0\n";
    fs << "POINTS " << pHypothesis->aConfirmedPoints.size() << "\n";
    fs << "DATA ascii\n";
 
    Vec3d vCenter = pHypothesis->vCenter;
    for (const CHypothesisPoint* p : pHypothesis->aConfirmedPoints)
    {
        uint32_t rgb = 0;
        rgb |= static_cast<uint8_t>(255.0f * p->fColorR * p->fIntensity) << 16;
        rgb |= static_cast<uint8_t>(255.0f * p->fColorG * p->fIntensity) << 8;
        rgb |= static_cast<uint8_t>(255.0f * p->fColorB * p->fIntensity);
 
        fs << p->vPosition.x - vCenter.x << " " << p->vPosition.y - vCenter.y << " " << p->vPosition.z - vCenter.z << " ";
        fs << rgb << "\n";
    }
 
    fs.close();
    */
}
 
void
CObjectRecognition::SaveObjectDescriptorRGBD(const CObjectHypothesis* pHypothesis,
                                             const std::string sObjectName,
                                             const int nDescriptorNumber)
{
    setlocale(LC_NUMERIC, "C");
    //printf ("Locale is: %s\n", setlocale(LC_NUMERIC, NULL));
 
    // generate filename and create file
    std::string sFileName = OLP_OBJECT_LEARNING_DIR;
    sFileName.append(sObjectName);
    std::string sFileNamePointsOnly = sFileName;
    std::string sFileNamePointsOnlyNoncentered = sFileName;
    sFileName.append("00.dat");
    sFileNamePointsOnly.append("-points00.dat");
    sFileNamePointsOnlyNoncentered.append("-points-noncentered00.dat");
    COLPTools::SetNumberInFileName(sFileName, nDescriptorNumber, 2);
    COLPTools::SetNumberInFileName(sFileNamePointsOnly, nDescriptorNumber, 2);
    COLPTools::SetNumberInFileName(sFileNamePointsOnlyNoncentered, nDescriptorNumber, 2);
    ARMARX_VERBOSE_S << "Saving object descriptor. File name: " << sFileName.c_str();
    FILE* pFile = fopen(sFileName.c_str(), "wt");
    FILE* pFilePointsOnly = fopen(sFileNamePointsOnly.c_str(), "wt");
    FILE* pFilePointsOnlyNoncentered = fopen(sFileNamePointsOnlyNoncentered.c_str(), "wt");
 
 
    // save the confirmed points
    fprintf(pFile, "%ld \n", pHypothesis->aConfirmedPoints.size());
    Vec3d vCenter = pHypothesis->vCenter;
 
    for (size_t i = 0; i < pHypothesis->aConfirmedPoints.size(); i++)
    {
        CHypothesisPoint* pPoint = pHypothesis->aConfirmedPoints.at(i);
        fprintf(pFile,
                "%e %e %e   %e %e %e %e \n",
                pPoint->vPosition.x - vCenter.x,
                pPoint->vPosition.y - vCenter.y,
                pPoint->vPosition.z - vCenter.z,
                pPoint->fColorR,
                pPoint->fColorG,
                pPoint->fColorB,
                pPoint->fIntensity);
        fprintf(pFilePointsOnly,
                "%e %e %e \n",
                pPoint->vPosition.x - vCenter.x,
                pPoint->vPosition.y - vCenter.y,
                pPoint->vPosition.z - vCenter.z);
        fprintf(pFilePointsOnlyNoncentered,
                "%e %e %e \n",
                pPoint->vPosition.x,
                pPoint->vPosition.y,
                pPoint->vPosition.z);
    }
 
    // save the candidate points too
    if (false)
    {
        fprintf(pFile, "%ld \n", pHypothesis->aNewPoints.size());
 
        for (size_t i = 0; i < pHypothesis->aNewPoints.size(); i++)
        {
            CHypothesisPoint* pPoint = pHypothesis->aNewPoints.at(i);
            fprintf(pFile,
                    "%e %e %e   %e %e %e %e \n",
                    pPoint->vPosition.x - vCenter.x,
                    pPoint->vPosition.y - vCenter.y,
                    pPoint->vPosition.z - vCenter.z,
                    pPoint->fColorR,
                    pPoint->fColorG,
                    pPoint->fColorB,
                    pPoint->fIntensity);
            fprintf(pFilePointsOnly,
                    "%e %e %e \n",
                    pPoint->vPosition.x - vCenter.x,
                    pPoint->vPosition.y - vCenter.y,
                    pPoint->vPosition.z - vCenter.z);
            fprintf(pFilePointsOnlyNoncentered,
                    "%e %e %e \n",
                    pPoint->vPosition.x,
                    pPoint->vPosition.y,
                    pPoint->vPosition.z);
        }
    }
 
    // close the file
    fclose(pFile);
    fclose(pFilePointsOnly);
    fclose(pFilePointsOnlyNoncentered);
}
 
bool
CObjectRecognition::LoadObjectDescriptorRGBD(const std::string sObjectName,
                                             const int nDescriptorNumber,
                                             CObjectHypothesis*& pHypothesis)
{
    setlocale(LC_NUMERIC, "C");
 
    // generate filename and open file
    std::string sFileName = OLP_OBJECT_LEARNING_DIR;
    sFileName.append(sObjectName);
    sFileName.append("00.dat");
    COLPTools::SetNumberInFileName(sFileName, nDescriptorNumber, 2);
    FILE* pFile = fopen(sFileName.c_str(), "rt");
 
    if (pFile == NULL)
    {
        pFile = fopen(sFileName.c_str(), "rt");
 
        if (pFile == NULL)
        {
            ARMARX_VERBOSE_S << "Cannot open object file: " << sFileName.c_str();
            pHypothesis = NULL;
            return false;
        }
    }
 
    pHypothesis = new CObjectHypothesis();
    pHypothesis->eType = CObjectHypothesis::eRGBD;
    Math3d::SetVec(pHypothesis->vCenter, 0, 0, 0);
 
    // read the confirmed points
    size_t nNumPoints;
    int dummy = fscanf(pFile, "%lu", &nNumPoints);
 
    for (size_t i = 0; i < nNumPoints; i++)
    {
        CHypothesisPoint* pPoint = new CHypothesisPoint();
        pPoint->ePointType = CHypothesisPoint::eDepthMapPoint;
        dummy += fscanf(
            pFile, "%e %e %e", &pPoint->vPosition.x, &pPoint->vPosition.y, &pPoint->vPosition.z);
        dummy += fscanf(pFile,
                        "%e %e %e %e \n",
                        &pPoint->fColorR,
                        &pPoint->fColorG,
                        &pPoint->fColorB,
                        &pPoint->fIntensity);
        pHypothesis->aConfirmedPoints.push_back(pPoint);
    }
 
    // read the candidate points
    dummy += fscanf(pFile, "%lu", &nNumPoints);
 
    for (size_t i = 0; i < nNumPoints; i++)
    {
        CHypothesisPoint* pPoint = new CHypothesisPoint();
        pPoint->ePointType = CHypothesisPoint::eDepthMapPoint;
        dummy += fscanf(
            pFile, "%e %e %e", &pPoint->vPosition.x, &pPoint->vPosition.y, &pPoint->vPosition.z);
        dummy += fscanf(pFile,
                        "%e %e %e %e \n",
                        &pPoint->fColorR,
                        &pPoint->fColorG,
                        &pPoint->fColorB,
                        &pPoint->fIntensity);
        pHypothesis->aNewPoints.push_back(pPoint);
    }
 
    return true;
}
 
void
CObjectRecognition::FindPossibleObjectLocations(const CObjectHypothesis* pHypothesis,
                                                const CByteImage* pHSVImage,
                                                const std::vector<CHypothesisPoint*>& aScenePoints,
                                                const CCalibration* calibration,
                                                std::vector<Vec3d>& aPossibleLocations,
                                                const int desiredNumberOfLocations)
{
    const int width = pHSVImage->width;
    const int height = pHSVImage->height;
 
    // detect possible locations of the object
    std::vector<float> hueHistogram, saturationHistogram;
    COLPTools::CreateHueAndSaturationHistogram(pHypothesis, hueHistogram, saturationHistogram);
 
    CByteImage* probabilityImage = new CByteImage(width, height, CByteImage::eGrayScale);
    FindSimilarRegionsWithHueAndSaturationHistogram(
        pHSVImage, hueHistogram, saturationHistogram, probabilityImage);
 
    //ImageProcessor::Spread(probabilityImage, probabilityImage);
    ::ImageProcessor::HistogramStretching(
        probabilityImage, probabilityImage, 0.7f, 1.0f); // 0.85, 1.0
 
 
    // calculate median in x and y direction
 
    std::vector<long> rowSums(height, 0);
    std::vector<long> columnSums(width, 0);
 
    long sum = 0;
 
    for (int i = 0; i < height; i++)
    {
        for (int j = 0; j < width; j++)
        {
            rowSums[i] += probabilityImage->pixels[i * width + j];
            columnSums[j] += probabilityImage->pixels[i * width + j];
            sum += probabilityImage->pixels[i * width + j];
        }
    }
 
    sum /= 2;
 
    for (int i = 1; i < height; i++)
    {
        rowSums[i] += rowSums[i - 1];
    }
 
    for (int j = 1; j < width; j++)
    {
        columnSums[j] += columnSums[j - 1];
    }
 
    int x = 0;
 
    while ((columnSums[x] < sum) && (x < width))
    {
        x++;
    }
 
    int y = 0;
 
    while ((rowSums[y] < sum) && (y < height))
    {
        y++;
    }
 
    Vec2d vMedian = {(float)x, (float)y};
 
 
    // cluster probable areas
 
    std::vector<Vec3d> samplePoints;
 
    for (int i = 0; i < height; i++)
    {
        for (int j = 0; j < width; j++)
        {
            if (rand() % 256 < probabilityImage->pixels[i * width + j])
            {
                if (rand() % 100 < 40 + 10 * OLP_EFFORT_POINTCLOUD_MATCHING)
                {
                    Vec3d newVec = {(float)j, (float)i, 0};
                    samplePoints.push_back(newVec);
                }
            }
        }
    }
 
    std::vector<std::vector<Vec3d>> clusters;
    std::vector<std::vector<int>> oldIndices;
 
    if (OLP_EFFORT_POINTCLOUD_MATCHING > 1)
    {
        COLPTools::ClusterXMeans(
            samplePoints, 1, 5, OLP_CLUSTERING_FACTOR_OBJECT_LOCALIZATION, clusters, oldIndices);
    }
    else
    {
        COLPTools::ClusterXMeans(samplePoints,
                                 1,
                                 4,
                                 0.6 * OLP_CLUSTERING_FACTOR_OBJECT_LOCALIZATION,
                                 clusters,
                                 oldIndices);
    }
 
    std::vector<Vec2d> locationCandidates;
    locationCandidates.push_back(vMedian);
 
    if (clusters.size() > 1)
    {
        for (size_t i = 0; i < clusters.size(); i++)
        {
            Vec3d vClusterCenter3D;
            float fTemp;
            COLPTools::GetMeanAndVariance(clusters.at(i), vClusterCenter3D, fTemp);
            Vec2d vClusterCenter2D = {vClusterCenter3D.x, vClusterCenter3D.y};
            locationCandidates.push_back(vClusterCenter2D);
            //COLPTools::DrawCross(probabilityImage, vClusterCenter2D.x, vClusterCenter2D.y, 50);
        }
    }
 
    int numAttempts = 0;
 
    while ((int)locationCandidates.size() < desiredNumberOfLocations && numAttempts < 10000)
    {
        int u = rand() % probabilityImage->width;
        int v = rand() % probabilityImage->height;
 
        if (probabilityImage->pixels[v * probabilityImage->width + u] > rand() % 500)
        {
            Vec2d pos = {(float)u, (float)v};
            locationCandidates.push_back(pos);
        }
 
        numAttempts++;
    }
 
 
    // visualize
    if (OLP_MAKE_INTERMEDIATE_SCREENSHOTS)
    {
        for (size_t n = 0; n < locationCandidates.size(); n++)
        {
            COLPTools::DrawCross(
                probabilityImage, locationCandidates.at(n).x, locationCandidates.at(n).y, 50);
        }
 
        std::string filename = OLP_SCREENSHOT_PATH;
        filename += "prob.bmp";
        probabilityImage->SaveToFile(filename.c_str());
    }
 
 
    // calculate 3D positions
    for (size_t n = 0; n < locationCandidates.size(); n++)
    {
        // calculate the 3D line defined by the 2D image point, and find the closest point in the scene point cloud
        Vec3d point1, point2, direction, estimatedPosition, temp1, temp2;
        calibration->ImageToWorldCoordinates(locationCandidates.at(n), point1, 100.0f, false);
        calibration->ImageToWorldCoordinates(locationCandidates.at(n), point2, 200.0f, false);
        Math3d::SubtractVecVec(point1, point2, direction);
        Math3d::NormalizeVec(direction);
        float fDist, fMinDist = 10000, fBestZ = 500;
 
        for (size_t i = 0; i < aScenePoints.size(); i++)
        {
            if (Math3d::Length(aScenePoints.at(i)->vPosition) > 0)
            {
                Math3d::SubtractVecVec(aScenePoints.at(i)->vPosition, point1, temp1);
                Math3d::CrossProduct(temp1, direction, temp2);
                fDist = Math3d::Length(temp2);
 
                if (fDist < fMinDist)
                {
                    fMinDist = fDist;
                    fBestZ = aScenePoints.at(i)->vPosition.z;
                }
            }
        }
 
        // get the point corresponding to the pixel with the same distance to the camera as the closest scene point
        calibration->ImageToWorldCoordinates(
            locationCandidates.at(n), estimatedPosition, fBestZ, false);
 
        //ARMARX_VERBOSE_S << "(%d, %d)   (%.1f, %.1f, %.1f)\n", locationCandidates.at(n).x, locationCandidates.at(n).y, estimatedPosition.x, estimatedPosition.y, estimatedPosition.z);
        aPossibleLocations.push_back(estimatedPosition);
    }
 
    //probabilityImage->SaveToFile("/homes/staff/schieben/datalog/prob.bmp");
    delete probabilityImage;
}
 
void
CObjectRecognition::FindSimilarRegionsWithHueAndSaturationHistogram(
    const CByteImage* pHSVImage,
    const std::vector<float>& aHueHistogram,
    const std::vector<float>& aSaturationHistogram,
    CByteImage* pProbabilityImage)
{
    const int nNumScales = 2;
    const float fVisualizationFactor = 0.33f * 255.0f / (float)nNumScales;
    const int nMinNumSubdivisionsX = 4;
 
    for (int i = 0; i < OLP_IMG_WIDTH * OLP_IMG_HEIGHT; i++)
    {
        pProbabilityImage->pixels[i] = 0;
    }
 
    std::vector<float> aHueHistogramWindow;
    aHueHistogramWindow.resize(aHueHistogram.size());
    std::vector<float> aSaturationHistogramWindow;
    aSaturationHistogramWindow.resize(aSaturationHistogram.size());
 
    for (int n = 0; n < nNumScales; n++)
    {
        //const int nNumSubdivisions = nMinNumNumSubdivisions << n;
        const int nNumSubdivisionsX = (int)((double)nMinNumSubdivisionsX * pow(sqrt(2), n));
        const int nNumSubdivisionsY =
            (int)(3.0 / 4.0 * (double)nMinNumSubdivisionsX * pow(sqrt(2), n));
        const int nWindowSizeX = OLP_IMG_WIDTH / nNumSubdivisionsX;
        const int nWindowSizeY = OLP_IMG_HEIGHT / nNumSubdivisionsY;
        int nMinX, nMaxX, nMinY, nMaxY;
 
        const int nWindowSizeX_3 = nWindowSizeX / 3;
        const int nWindowSizeY_3 = nWindowSizeY / 3;
 
        for (int i = 0; i < nNumSubdivisionsY; i++)
        {
            for (int j = 0; j < nNumSubdivisionsX; j++)
            {
                // shift window
                for (int m = -1; m <= 1; m++)
                {
                    nMinX = j * nWindowSizeX + m * nWindowSizeX_3;
                    nMaxX = (j + 1) * nWindowSizeX + m * nWindowSizeX_3;
                    nMinY = i * nWindowSizeY + m * nWindowSizeY_3;
                    nMaxY = (i + 1) * nWindowSizeY + m * nWindowSizeY_3;
 
                    if (nMinX < 0)
                    {
                        nMinX = 0;
                    }
 
                    if (nMinY < 0)
                    {
                        nMinY = 0;
                    }
 
                    if (nMaxX > OLP_IMG_WIDTH)
                    {
                        nMaxX = OLP_IMG_WIDTH;
                    }
 
                    if (nMaxY > OLP_IMG_HEIGHT)
                    {
                        nMaxY = OLP_IMG_HEIGHT;
                    }
 
                    COLPTools::CreateHueAndSaturationHistogramInWindow(pHSVImage,
                                                                       nMinX,
                                                                       nMinY,
                                                                       nMaxX,
                                                                       nMaxY,
                                                                       aHueHistogramWindow,
                                                                       aSaturationHistogramWindow);
 
                    const float fHistogramDistance =
                        0.5f *
                            COLPTools::GetHistogramDistanceX2(aHueHistogram, aHueHistogramWindow) +
                        0.5f * COLPTools::GetHistogramDistanceX2(aSaturationHistogram,
                                                                 aSaturationHistogramWindow);
                    const float fCorrelation = 1.0f - (0.25f * fHistogramDistance *
                                                       fHistogramDistance); // distance in [0,2]
 
                    // visualize in probability image
                    unsigned char nCorrelationValueVisualization =
                        (unsigned char)(fVisualizationFactor * fCorrelation);
 
                    for (int k = nMinY; k < nMaxY; k++)
                    {
                        for (int l = nMinX; l < nMaxX; l++)
                        {
                            pProbabilityImage->pixels[k * OLP_IMG_WIDTH + l] +=
                                nCorrelationValueVisualization;
                        }
                    }
                }
            }
        }
    }
}
 
void
CObjectRecognition::FindObjectRGBD(const CObjectHypothesis* pHypothesis,
                                   const CByteImage* pHSVImage,
                                   const std::vector<CHypothesisPoint*>& aScenePoints,
                                   const CCalibration* calibration,
                                   const Vec3d upwardsVector,
                                   Vec3d& vTranslation,
                                   Mat3d& mOrientation,
                                   float& distance,
                                   float& fProbability,
                                   CByteImage* pResultImage)
{
    // get possible object locations based on color region similarity
    std::vector<Vec3d> aPossibleLocations;
    FindPossibleObjectLocations(
        pHypothesis, pHSVImage, aScenePoints, calibration, aPossibleLocations);
 
    // set up the point cloud registration
    CPointCloudRegistration* pPointCloudRegistration = new CPointCloudRegistration();
    pPointCloudRegistration->SetScenePointcloud(aScenePoints);
 
 
    // try to match the object at the promising locations
    distance = pPointCloudRegistration->EstimateTransformation(pHypothesis,
                                                               mOrientation,
                                                               vTranslation,
                                                               upwardsVector,
                                                               OLP_EFFORT_POINTCLOUD_MATCHING,
                                                               &aPossibleLocations);
    delete pPointCloudRegistration;
 
    fProbability = 1.0f / (1.0f + 0.12f * distance);
 
    ARMARX_IMPORTANT_S << "Distance: " << distance << ", probability: " << fProbability;
    ARMARX_VERBOSE_S << "Translation: ( " << vTranslation.x << ", " << vTranslation.y << ", "
                     << vTranslation.z << ")";
 
 
    // visualize
    if (pResultImage)
    {
        std::vector<CHypothesisPoint*> aObjectPoints;
 
        for (size_t i = 0; i < pHypothesis->aConfirmedPoints.size(); i++)
        {
            aObjectPoints.push_back(pHypothesis->aConfirmedPoints.at(i)->GetCopy());
            Math3d::MulMatVec(mOrientation,
                              aObjectPoints.back()->vPosition,
                              vTranslation,
                              aObjectPoints.back()->vPosition);
        }
 
        unsigned char r, g, b;
 
        if (fProbability > 0.5f)
        {
            r = 0;
            g = 200;
            b = 0;
        }
        else
        {
            r = 100;
            g = 20;
            b = 20;
        }
 
        Vec2d vPos2D;
 
        for (size_t i = 0; i < aObjectPoints.size(); i++)
        {
            calibration->WorldToImageCoordinates(aObjectPoints.at(i)->vPosition, vPos2D, false);
 
            if (vPos2D.x > 1 && vPos2D.y > 1 && vPos2D.x < OLP_IMG_WIDTH - 2 &&
                vPos2D.y < OLP_IMG_HEIGHT - 2)
            {
                for (int j = -1; j <= 1; j++)
                {
                    pResultImage
                        ->pixels[3 * (((int)vPos2D.y + j) * OLP_IMG_WIDTH + (int)vPos2D.x)] = r;
                    pResultImage
                        ->pixels[3 * (((int)vPos2D.y + j) * OLP_IMG_WIDTH + (int)vPos2D.x) + 1] = g;
                    pResultImage
                        ->pixels[3 * (((int)vPos2D.y + j) * OLP_IMG_WIDTH + (int)vPos2D.x) + 2] = b;
 
                    pResultImage->pixels[3 * ((int)vPos2D.y * OLP_IMG_WIDTH + (int)vPos2D.x + j)] =
                        r;
                    pResultImage
                        ->pixels[3 * ((int)vPos2D.y * OLP_IMG_WIDTH + (int)vPos2D.x + j) + 1] = g;
                    pResultImage
                        ->pixels[3 * ((int)vPos2D.y * OLP_IMG_WIDTH + (int)vPos2D.x + j) + 2] = b;
                }
            }
        }
 
        // cleanup
        for (size_t i = 0; i < aObjectPoints.size(); i++)
        {
            delete aObjectPoints.at(i);
        }
    }
}
 
void
CObjectRecognition::FindAllObjectsRGBD(const CByteImage* pHSVImage,
                                       const CByteImage* pRGBImage,
                                       const std::vector<CHypothesisPoint*>& aScenePoints,
                                       const CCalibration* calibration,
                                       const Vec3d upwardsVector,
                                       std::vector<std::string>& aNames,
                                       std::vector<Vec3d>& aPositions,
                                       std::vector<Mat3d>& aOrientations,
                                       std::vector<float>& aProbabilities)
{
    // load the names of the known objects, and the numbers of the respective descriptor file to be used
    std::string sObjectNamesFileName = OLP_OBJECT_LEARNING_DIR;
    sObjectNamesFileName += "names.txt";
    FILE* pObjectNamesFile = fopen(sObjectNamesFileName.c_str(), "rt");
 
    if (pObjectNamesFile == NULL)
    {
        ARMARX_VERBOSE_S << "Opening file with object names failed!";
        return;
    }
 
    int nNumObjects;
    int dummy = fscanf(pObjectNamesFile, "%d", &nNumObjects);
    char pcObjName[100];
    int nDescriptorNumber;
    std::string sObjName;
    std::vector<int> aDescriptorNumbers;
    aNames.clear();
 
    for (int i = 0; i < nNumObjects; i++)
    {
        dummy += fscanf(pObjectNamesFile, "%s", pcObjName);
        dummy += fscanf(pObjectNamesFile, "%d", &nDescriptorNumber);
        sObjName = pcObjName;
        aNames.push_back(sObjName);
        aDescriptorNumbers.push_back(nDescriptorNumber);
    }
 
    aPositions.resize(nNumObjects);
    aOrientations.resize(nNumObjects);
    aProbabilities.resize(nNumObjects);
    std::vector<float> aDistances;
    aDistances.resize(nNumObjects);
    fclose(pObjectNamesFile);
 
    // debug: result images
    CByteImage** pResultImages = new CByteImage*[nNumObjects];
 
    for (int i = 0; i < nNumObjects; i++)
    {
        pResultImages[i] = new CByteImage(OLP_IMG_WIDTH, OLP_IMG_HEIGHT, CByteImage::eRGB24);
        ImageProcessor::CopyImage(pRGBImage, pResultImages[i]);
    }
 
    // try to recognize the objects
    for (int i = 0; i < nNumObjects; i++)
    {
        CObjectHypothesis* pHypothesis;
        ARMARX_VERBOSE_S << aNames.at(i) << " (" << i << ")";
        LoadObjectDescriptorRGBD(aNames.at(i), aDescriptorNumbers.at(i), pHypothesis);
        FindObjectRGBD(pHypothesis,
                       pHSVImage,
                       aScenePoints,
                       calibration,
                       upwardsVector,
                       aPositions.at(i),
                       aOrientations.at(i),
                       aDistances.at(i),
                       aProbabilities.at(i),
                       pResultImages[i]);
        delete pHypothesis;
    }
 
 
    // debug: write recognition results to file
    for (int i = 0; i < nNumObjects; i++)
    {
        std::string sResImgName = "/home/staff/schieben/datalog/reco00.bmp";
        COLPTools::SetNumberInFileName(sResImgName, i, 2);
        pResultImages[i]->SaveToFile(sResImgName.c_str());
        delete pResultImages[i];
    }
 
    delete[] pResultImages;
}
 
void
CObjectRecognition::FindObjectWithManyDescriptorsRGBD(
    const CByteImage* pHSVImage,
    const CByteImage* pRGBImage,
    const std::vector<CHypothesisPoint*>& aScenePoints,
    const CCalibration* calibration,
    const Vec3d upwardsVector,
    const std::string objectName,
    const int numDescriptors)
{
    std::string directory = OLP_OBJECT_LEARNING_DIR;
    std::string sFileName = directory + "reco_dist_" + objectName + ".txt";
    FILE* pFile = fopen(sFileName.c_str(), "a");
 
    CByteImage* resultImage = new CByteImage(pRGBImage);
    ImageProcessor::CopyImage(pRGBImage, resultImage);
 
    Vec3d position;
    Mat3d orientation;
    float probability = -1;
    float distance = -1;
 
    for (int i = 1; i <= numDescriptors; i++)
    {
        CObjectHypothesis* pHypothesis;
        ARMARX_INFO_S << "Recognizing " << objectName << " (descriptor " << i << ")";
        LoadObjectDescriptorRGBD(objectName, i, pHypothesis);
        FindObjectRGBD(pHypothesis,
                       pHSVImage,
                       aScenePoints,
                       calibration,
                       upwardsVector,
                       position,
                       orientation,
                       distance,
                       probability,
                       resultImage);
        fprintf(pFile, "%e ", distance);
        delete pHypothesis;
    }
 
    std::string sResImgName = directory + "reco_result.bmp";
    resultImage->SaveToFile(sResImgName.c_str());
 
    fprintf(pFile, "\n");
    fclose(pFile);
}
 
void
CObjectRecognition::SaveObjectDescriptorRFCH(const CByteImage* pRGBImage,
                                             const CByteImage* pObjectMask,
                                             const std::string sObjectName,
                                             const int nDescriptorNumber)
{
#ifdef OLP_USE_EARLYVISION
    ///////////////////////////////////////////////////////////
    // parameters
    ///////////////////////////////////////////////////////////
    const int nNumberClusters =
        30; // number of clusters to use. More clusters means more descriptive but too much cluster means overfitting with varying lighting conditions.
    const float fErrorThreshold =
        0.5; // threshold for matching histograms. Should be reduced if two many matches arise
    const float fMatchThreshold = 0.5; // threshold for assigning values to clusters
 
    ////////////////////////////////
    // create signature calculator
    ////////////////////////////////
    CRFCSignatureCalculator* pCalc = new CRFCSignatureCalculator(nNumberClusters);
 
    pCalc->SetUseZeroOffset(false);
    pCalc->SetErrorThreshold(fErrorThreshold);
    pCalc->SetCorrespondanceDistanceFactor(fMatchThreshold);
    pCalc->SetMinConsidered(0.5);
 
    ///////////////////////////////////////////////////////////
    // setup dimension. You can use any dimension from Base/Features/RFCH/Descriptors/
    ///////////////////////////////////////////////////////////
    CHueDescriptor* pHue = new CHueDescriptor(0, 10, 255, 10, 255);
    CHueLaplacianDescriptor* pHueL = new CHueLaplacianDescriptor(0, 10, 255, 10, 255);
    pCalc->AddDimension(pHue);
    pCalc->AddDimension(pHueL);
 
    pCalc->Init(OLP_IMG_WIDTH, OLP_IMG_HEIGHT, 30, 20); // number windows: 30 x 20 y
 
    // train query
    CByteImage* pRGBImageCopy = new CByteImage(pRGBImage);
    ImageProcessor::CopyImage(pRGBImage, pRGBImageCopy);
 
    if (pRGBImageCopy->width != pRGBImage->width)
    {
        ARMARX_VERBOSE_S << "\nBuuuuugs!\n\n");
    }
 
    CByteImage* pObjectMaskCopy = new CByteImage(pObjectMask);
    ImageProcessor::CopyImage(pObjectMask, pObjectMaskCopy);
    CRFCSignatureFeatureEntry* pFeatureDescriptor =
        (CRFCSignatureFeatureEntry*)pCalc->Train(pRGBImageCopy, pObjectMaskCopy);
    CRFCSignatureCalculator::NormalizeHistogram(pFeatureDescriptor, 10000); // always do that
 
    // generate filename, create file, write descriptor
    std::string sFileName = OLP_OBJECT_LEARNING_DIR;
    sFileName.append(sObjectName);
    sFileName.append("00.rfc");
    COLPTools::SetNumberInFileName(sFileName, nDescriptorNumber, 2);
    ARMARX_VERBOSE_S << "\nSaving object descriptor. File name: %s\n", sFileName.c_str());
    FILE* pFile = fopen(sFileName.c_str(), "wt");
 
    if (!pFeatureDescriptor->WriteToFile(pFile))
    {
        ARMARX_VERBOSE_S << "\nError writing to file!\n\n");
    }
 
    fclose(pFile);
#endif
}
 
void
CObjectRecognition::LoadObjectDescriptorRFCH(const std::string sObjectName,
                                             const int nDescriptorNumber,
                                             CRFCSignatureFeatureEntry*& pFeatureDescriptor)
{
#ifdef OLP_USE_EARLYVISION
    // generate filename
    std::string sFileName = OLP_OBJECT_LEARNING_DIR;
    sFileName.append(sObjectName);
    sFileName.append("00.rfc");
    COLPTools::SetNumberInFileName(sFileName, nDescriptorNumber, 2);
    FILE* pFile = fopen(sFileName.c_str(), "rt");
 
    if (pFile == NULL)
    {
        ARMARX_VERBOSE_S << "\nCannot open object file: %s\n\n", sFileName.c_str());
        pFeatureDescriptor = NULL;
    }
    else
    {
        pFeatureDescriptor->ReadFromFile(pFile);
    }
 
#endif
}
 
void
CObjectRecognition::FindObjectRFCH(CRFCSignatureFeatureEntry* pFeatureDescriptor,
                                   CByteImage* pRGBImage,
                                   const std::vector<CHypothesisPoint*>& aScenePoints,
                                   const CCalibration* calibration,
                                   Vec3d& vPosition,
                                   Mat3d& mOrientation,
                                   float& fProbability,
                                   CByteImage* pResultImage)
{
#ifdef OLP_USE_EARLYVISION
    ///////////////////////////////////////////////////////////
    // parameters
    ///////////////////////////////////////////////////////////
    const int nNumberClusters =
        30; // number of clusters to use. More clusters means more descriptive but too much cluster means overfitting with varying lighting conditions.
    const float fErrorThreshold =
        0.5; // threshold for matching histograms. Should be reduced if two many matches arise
    const float fMatchThreshold = 0.5; // threshold for assigning values to clusters
    float fGrowThreshold =
        0.8; // grow threshold of the additive region image. If the detected area is too big, reduce this value. Otherwise increase.
 
    ////////////////////////////////
    // create signature calculator
    ////////////////////////////////
    CRFCSignatureCalculator* pCalc = new CRFCSignatureCalculator(nNumberClusters);
 
    pCalc->SetUseZeroOffset(false);
    pCalc->SetErrorThreshold(fErrorThreshold);
    pCalc->SetCorrespondanceDistanceFactor(fMatchThreshold);
    pCalc->SetMinConsidered(0.5);
 
    ///////////////////////////////////////////////////////////
    // setup dimension. You can use any dimension from Base/Features/RFCH/Descriptors/
    ///////////////////////////////////////////////////////////
    CHueDescriptor* pHue = new CHueDescriptor(0, 10, 255, 10, 255);
    CHueLaplacianDescriptor* pHueL = new CHueLaplacianDescriptor(0, 10, 255, 10, 255);
    pCalc->AddDimension(pHue);
    pCalc->AddDimension(pHueL);
 
    pCalc->Init(OLP_IMG_WIDTH, OLP_IMG_HEIGHT, 30, 20); // number windows: 30 x 20 y
 
 
    // match in scene
    CFeatureEntry* pfQuery = pFeatureDescriptor;
    pCalc->PrepareMatching(pRGBImage, NULL, &pfQuery, 1); // query one feature
    vector<TRFCMatch> aMatches = pCalc->Match(
        pfQuery, 2, 2, 4, 4); // 2,2: min number of windows x,y 4,4: max number of windows x,y
 
    ARMARX_VERBOSE_S << "%ld matches extracted\n", aMatches.size());
 
    // apply ARI
    int nBoarderOffset = pCalc->GetBoarderOffset();
    int nNumberWindowsX, nNumberWindowsY, nWindowPixelsX, nWindowPixelsY;
 
    pCalc->GetNumberWindows(nNumberWindowsX, nNumberWindowsY); // get number of search windows
    pCalc->GetWindowSize(nWindowPixelsX, nWindowPixelsY); // get size of minimum window size
 
    CAdditiveRegionImage* pARI = new CAdditiveRegionImage(
        nNumberWindowsX, nNumberWindowsY, nWindowPixelsX, nWindowPixelsY, fGrowThreshold);
    pARI->setOffsets(nBoarderOffset, nBoarderOffset);
 
    vector<TRFCMatch>::iterator iter = aMatches.begin();
    int nID = 0;
    std::vector<TRegionListEntry> aRegionList;
 
    while (iter != aMatches.end())
    {
        TRegionListEntry entry;
        entry.region = iter->region;
        entry.fActivation = iter->fMatch;
        entry.id = nID;
        entry.bUse = true;
 
        aRegionList.push_back(entry);
 
        nID++;
        iter++;
    }
 
    pARI->set(aRegionList);
    ARMARX_VERBOSE_S << "Evaluation ARI (GrowThreshold: %f) ...\n", fGrowThreshold);
    std::vector<TResultRegion> aRegions = pARI->calculateRegions();
 
    ARMARX_VERBOSE_S << "Extracted %ld matching regions\n", aRegions.size());
 
    // visualize
 
    if (pResultImage != NULL)
    {
        pARI->visualize(pResultImage, aRegions);
        pResultImage->SaveToFile("/home/staff/schieben/datalog/reco.bmp");
    }
 
 
    // find object center
    if (aRegionList.size() > 0)
    {
        // calculate a 3D position: calculate the 3D line defined by the 2D image point, and find the closest point in the scene point cloud
        Vec2d vCenter2D = aRegionList.at(0).region.centroid;
        Vec3d vPoint1, vPoint2, vDirection, vTemp1, vTemp2;
        calibration->ImageToWorldCoordinates(vCenter2D, vPoint1, 100.0f, false);
        calibration->ImageToWorldCoordinates(vCenter2D, vPoint2, 200.0f, false);
        Math3d::SubtractVecVec(vPoint1, vPoint2, vDirection);
        Math3d::NormalizeVec(vDirection);
        float fDist, fMinDist = 10000, fBestZ = 500;
 
        for (size_t i = 0; i < aScenePoints.size(); i++)
        {
            Math3d::SubtractVecVec(aScenePoints.at(i)->vPosition, vPoint1, vTemp1);
            Math3d::CrossProduct(vTemp1, vDirection, vTemp2);
            fDist = Math3d::Length(vTemp2);
 
            if (fDist < fMinDist)
            {
                fMinDist = fDist;
                fBestZ = aScenePoints.at(i)->vPosition.z;
            }
        }
 
        // get the point corresponding to the pixel with the same distance to the camera as the closest scene point
        calibration->ImageToWorldCoordinates(vCenter2D, vPosition, fBestZ, false);
 
        Math3d::SetMat(mOrientation, Math3d::unit_mat);
        fProbability = aRegionList.at(0).fActivation;
    }
    else
    {
        Math3d::SetVec(vPosition, 0, 0, 0);
        Math3d::SetMat(mOrientation, Math3d::unit_mat);
        fProbability = 0;
    }
 
#endif
}
 
void
CObjectRecognition::FindAllObjectsRFCH(const CByteImage* pRGBImage,
                                       const CCalibration* calibration,
                                       const std::vector<CHypothesisPoint*>& aScenePoints,
                                       std::vector<std::string>& aNames,
                                       std::vector<Vec3d>& aPositions,
                                       std::vector<Mat3d>& aOrientations,
                                       std::vector<float>& aProbabilities)
{
#ifdef OLP_USE_EARLYVISION
    CByteImage* pResultImage = new CByteImage(OLP_IMG_WIDTH, OLP_IMG_HEIGHT, CByteImage::eRGB24);
    ImageProcessor::CopyImage(pRGBImage, pResultImage);
    CByteImage* pRGBImageCopy = new CByteImage(pRGBImage);
    ImageProcessor::CopyImage(pRGBImage, pRGBImageCopy);
 
    // load the names of the known objects, and the numbers of the respective descriptor file to be used
    std::string sObjectNamesFileName = OLP_OBJECT_LEARNING_DIR;
    sObjectNamesFileName += "names.txt";
    FILE* pObjectNamesFile = fopen(sObjectNamesFileName.c_str(), "rt");
 
    if (pObjectNamesFile == NULL)
    {
        ARMARX_VERBOSE_S << "\nOpening file with object names failed!\n\n");
        return;
    }
 
    int nNumObjects;
    fscanf(pObjectNamesFile, "%d", &nNumObjects);
    char pcObjName[100];
    int nDescriptorNumber;
    std::string sObjName;
    std::vector<int> aDescriptorNumbers;
    aNames.clear();
 
    for (int i = 0; i < nNumObjects; i++)
    {
        fscanf(pObjectNamesFile, "%s", pcObjName);
        fscanf(pObjectNamesFile, "%d", &nDescriptorNumber);
        sObjName = pcObjName;
        aNames.push_back(sObjName);
        aDescriptorNumbers.push_back(nDescriptorNumber);
    }
 
    aPositions.resize(nNumObjects);
    aOrientations.resize(nNumObjects);
    aProbabilities.resize(nNumObjects);
    fclose(pObjectNamesFile);
 
    // try to recognize the objects
    const int nNumberClusters = 30;
    CRFCSignatureFeatureEntry* pFeatureDescriptor =
        new CRFCSignatureFeatureEntry(2, nNumberClusters);
 
    for (int i = 0; i < nNumObjects; i++)
    {
        ARMARX_VERBOSE_S << "\n%s\n", aNames.at(i).c_str());
        LoadObjectDescriptorRFCH(aNames.at(i), aDescriptorNumbers.at(i), pFeatureDescriptor);
        FindObjectRFCH(pFeatureDescriptor,
                       pRGBImageCopy,
                       aScenePoints,
                       calibration,
                       aPositions.at(i),
                       aOrientations.at(i),
                       aProbabilities.at(i),
                       pResultImage);
    }
 
    delete pFeatureDescriptor;
 
    pResultImage->SaveToFile("/home/staff/schieben/datalog/reco.bmp");
    delete pResultImage;
#endif
}