SegmentableTemplateRecognition.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::Component
* @author     Kai Welke <welke at kit dot edu>
* @copyright  2013 Humanoids Group, HIS, KIT
* @license    http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include "SegmentableTemplateRecognition.h"
 
#include <fstream>
 
// Core
#include <ArmarXCore/core/system/ArmarXDataPath.h>
 
// RobotState
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/Pose.h>
 
// IVT
#include <Helpers/helpers.h>
#include <Image/ByteImage.h>
#include <Image/ImageProcessor.h>
#include <Image/PrimitivesDrawer.h>
#include <Math/FloatMatrix.h>
 
// MemoryX
#include <ArmarXCore/observers/variant/TimestampVariant.h>
 
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/libraries/helpers/ObjectRecognitionHelpers/ObjectRecognitionWrapper.h>
 
// This file must be included after all Ice-related headers when Ice 3.5.x is used (e.g. with Ubuntu 14.04 or Debian 7.0)!
// GLContext.h indirectly includes X.h which does "#define None", colliding with IceUtil::None.
#include <boost/scope_exit.hpp>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
 
#include <gui/GLContext.h>
 
#pragma GCC diagnostic ignored "-Wmissing-field-initializers"
 
using namespace armarx;
using namespace visionx;
using namespace memoryx::EntityWrappers;
 
namespace
{
 
    struct HSVSegmentationParams
    {
        int hueValue;
        int hueTolerance;
        int saturationMin;
        int saturationMax;
        int valueMin;
        int valueMax;
 
        float minRatio = 0.0f;
    };
 
    HSVSegmentationParams
    getSegmentationParamsFromColor(CColorParameterSet const& colorParameters, ObjectColor color)
    {
        HSVSegmentationParams result = {};
        int const* colorArray = colorParameters.GetColorParameters(color);
        if (colorArray)
        {
            result.hueValue = colorArray[0];
            result.hueTolerance = colorArray[1];
            result.saturationMin = colorArray[2];
            result.saturationMax = colorArray[3];
            result.valueMin = colorArray[4];
            result.valueMax = colorArray[5];
        }
        else
        {
            ARMARX_WARNING << "Unknown color: " << color;
        }
        return result;
    }
 
    struct BlobDetector
    {
        BlobDetector(int width, int height) :
            segmented(width, height, CByteImage::eGrayScale),
            temp(width, height, CByteImage::eGrayScale),
            hsv(width, height, CByteImage::eRGB24)
        {
        }
 
        BlobDetector(BlobDetector const&) = delete;
        BlobDetector& operator=(BlobDetector const&) = delete;
 
        Object2DList
        findColoredRegions(CByteImage* inputImage,
                           int minPixelsPerRegion,
                           std::vector<HSVSegmentationParams>& segmentationParams,
                           int dilationCount = 2)
        {
            ::ImageProcessor::CalculateHSVImage(inputImage, &hsv);
 
            ::ImageProcessor::Zero(&segmented);
            for (HSVSegmentationParams const& segParam : segmentationParams)
            {
                ::ImageProcessor::FilterHSV(&hsv,
                                            &temp,
                                            segParam.hueValue,
                                            segParam.hueTolerance,
                                            segParam.saturationMin,
                                            segParam.saturationMax,
                                            segParam.valueMin,
                                            segParam.valueMax);
                ::ImageProcessor::Or(&segmented, &temp, &segmented);
            }
 
            for (int i = 0; i < dilationCount / 2; ++i)
            {
                ::ImageProcessor::Erode(&segmented, &temp);
                ::ImageProcessor::Dilate(&temp, &segmented);
            }
            for (int i = 0; i < dilationCount / 2; ++i)
            {
                ::ImageProcessor::Dilate(&segmented, &temp);
                ::ImageProcessor::Erode(&temp, &segmented);
            }
            if (dilationCount == 0)
            {
                ::ImageProcessor::Dilate(&segmented, &temp);
                ::ImageProcessor::Dilate(&temp, &segmented);
                ::ImageProcessor::Erode(&segmented, &temp);
                ::ImageProcessor::CopyImage(&temp, &segmented);
            }
 
            RegionList regions;
            ::ImageProcessor::FindRegions(&segmented, regions, minPixelsPerRegion);
 
            Object2DList objects;
            objects.reserve(regions.size());
            int objectId = 0;
            for (MyRegion const& region : regions)
            {
                Object2DEntry object;
                //object.color = color; // FIXME: We have changed to HSV seg parameters
                object.region = region;
 
                for (HSVSegmentationParams& seg : segmentationParams)
                {
                    if (seg.minRatio > 0.0f)
                    {
                        int count = 0;
 
                        int min_hue = (int)seg.hueValue - (int)seg.hueTolerance;
                        int max_hue = (int)seg.hueValue + (int)seg.hueTolerance;
 
                        if (min_hue < 0)
                        {
                            min_hue += 180;
                        }
 
                        if (max_hue >= 180)
                        {
                            max_hue -= 180;
                        }
 
                        for (int y = region.min_y; y < region.max_y; ++y)
                        {
                            for (int x = region.min_x; x < region.max_x; ++x)
                            {
                                int pixelIndex = y * hsv.width + x;
                                int h = hsv.pixels[pixelIndex * 3 + 0];
                                int s = hsv.pixels[pixelIndex * 3 + 1];
                                int v = hsv.pixels[pixelIndex * 3 + 2];
                                if (seg.saturationMin <= s && s <= seg.saturationMax &&
                                    seg.valueMin <= v && v <= seg.valueMax)
                                {
                                    if (max_hue >= min_hue)
                                    {
                                        if (min_hue <= h && h <= max_hue)
                                        {
                                            ++count;
                                        }
                                    }
                                    else
                                    {
                                        if (h <= max_hue || h >= min_hue)
                                        {
                                            ++count;
                                        }
                                    }
                                }
                            }
                        }
 
                        float ratio = float(count) / region.nPixels;
                        if (ratio < seg.minRatio)
                        {
                            ARMARX_VERBOSE << "Discarding region because ratio too small: "
                                           << ratio;
                            continue;
                        }
                    }
                }
 
                object.type = eCompactObject;
                object.sName = "CompactObject";
                object.id = objectId++;
 
                objects.push_back(object);
            }
            return objects;
        }
 
        CByteImage segmented;
        CByteImage temp;
        CByteImage hsv;
    };
 
    void
    CropImageToTemplate(CByteImage* inputImage, MyRegion const& region, CByteImage* outputImage)
    {
        int width = region.max_x - region.min_x + 1;
        int height = region.max_y - region.min_y + 1;
        int k = outputImage->width;
 
 
        if (width >= height)
        {
            int new_height = int((k * height) / float(width) + 0.5f);
            CByteImage temp_image(k, new_height, CByteImage::eGrayScale);
 
            ::ImageProcessor::Resize(inputImage, &temp_image, &region);
 
            const int nPixels = k * new_height;
            unsigned char* output = outputImage->pixels;
 
            memcpy(output, temp_image.pixels, nPixels);
 
            const int nTotalPixels = k * k;
 
            for (int i = nPixels; i < nTotalPixels; i++)
            {
                output[i] = 0;
            }
        }
        else
        {
            int new_width = int((k * width) / float(height) + 0.5f);
            CByteImage temp_image(new_width, k, CByteImage::eGrayScale);
 
            ::ImageProcessor::Resize(inputImage, &temp_image, &region);
 
            const unsigned char* input = temp_image.pixels;
            unsigned char* output = outputImage->pixels;
 
            for (int i = 0, offset = 0, offset2 = 0; i < k; i++)
            {
                int j;
 
                for (j = 0; j < new_width; j++, offset++, offset2++)
                {
                    output[offset] = input[offset2];
                }
 
                for (j = new_width; j < k; j++, offset++)
                {
                    output[offset] = 0;
                }
            }
        }
    }
 
} // namespace
 
SegmentableTemplateRecognition::SegmentableTemplateRecognition()
{
}
 
SegmentableTemplateRecognition::~SegmentableTemplateRecognition()
{
}
 
void
SegmentableTemplateRecognition::onInitObjectLocalizerProcessor()
{
    offeringTopic(getProperty<std::string>("DebugObserverName").getValue());
 
    paramMinPixelsPerRegion = getProperty<int>("MinPixelsPerRegion").getValue();
}
 
void
SegmentableTemplateRecognition::onExitObjectLocalizerProcessor()
{
}
 
std::vector<std::string>
getFileList(const std::string& path)
{
    std::vector<std::string> result;
    if (!path.empty())
    {
        namespace fs = std::filesystem;
 
        fs::path apk_path(path);
        fs::recursive_directory_iterator end;
 
        for (fs::recursive_directory_iterator i(apk_path); i != end; ++i)
        {
            const fs::path cp = (*i);
            result.push_back(cp.string());
        }
    }
    return result;
}
 
bool
SegmentableTemplateRecognition::initRecognizer()
{
    ARMARX_VERBOSE << "Initializing SegmentableTemplateRecognition";
 
    Eigen::Vector3f minPoint = getProperty<Eigen::Vector3f>("MinPoint").getValue();
    Eigen::Vector3f maxPoint = getProperty<Eigen::Vector3f>("MaxPoint").getValue();
 
    Math3d::SetVec(validResultBoundingBoxMin, minPoint(0), minPoint(1), minPoint(2));
    Math3d::SetVec(validResultBoundingBoxMax, maxPoint(0), maxPoint(1), maxPoint(2));
 
    maxEpipolarDistance = getProperty<float>("MaxEpipolarDistance").getValue();
    std::string colorParemeterFilename = getProperty<std::string>("ColorParameterFile").getValue();
 
    paramTemplateMatchThreshold = getProperty<float>("TemplateMatchThreshold");
    paramSizeRatioThreshold = getProperty<float>("SizeRatioThreshold");
    paramCorrelationThreshold = getProperty<float>("CorrelationThreshold");
    paramSingleInstance = getProperty<bool>("SingleInstance");
 
    if (!ArmarXDataPath::getAbsolutePath(colorParemeterFilename, colorParemeterFilename))
    {
        ARMARX_ERROR << "Could not find color parameter file in ArmarXDataPath: "
                     << colorParemeterFilename;
    }
 
    templatePath = getProperty<std::string>("TemplatePath").getValue();
    if (!ArmarXDataPath::getAbsolutePath(templatePath, templatePath))
    {
        ARMARX_ERROR << "Could not find template path file in ArmarXDataPath: " << templatePath;
    }
 
    ARMARX_IMPORTANT << "Looking in template path: " << templatePath;
    auto templateFiles = getFileList(templatePath);
    for (std::string const& templateFile : templateFiles)
    {
        if (!simox::alg::ends_with(templateFile, ".seg"))
        {
            continue;
        }
 
        SegmentableTemplateEntry entry;
 
        ARMARX_INFO << "Loading template: " << templateFile;
 
        FILE* file = fopen(templateFile.c_str(), "rb");
        BOOST_SCOPE_EXIT((file))
        {
            fclose(file);
        }
        BOOST_SCOPE_EXIT_END;
 
        fseek(file, 0L, SEEK_END);
        long fileSize = ftell(file);
        fseek(file, 0L, SEEK_SET);
 
        void* memory = operator new(fileSize);
        entry.data.reset(new (memory) SegmentableTemplateHeader);
        if (fileSize < (long)sizeof(SegmentableTemplateHeader))
        {
            ARMARX_WARNING << "File is too short for template header: " << templateFile
                           << " (expected size: " << sizeof(SegmentableTemplateHeader)
                           << ", but got size: " << fileSize << ")";
            continue;
        }
 
        long read = fread(memory, 1, fileSize, file);
        if (read != fileSize)
        {
            ARMARX_WARNING << "Could not read template file completely (expected size: " << fileSize
                           << ", but read: " << read << ")";
            continue;
        }
 
        std::string objectName = entry.data->name;
        {
            // Load model
            std::string modelFilename = templatePath + "/" + objectName + ".mat";
            entry.model.reset(new CFloatMatrix());
            if (!entry.model->LoadFromFile(modelFilename.c_str()))
            {
                ARMARX_WARNING << "Could not load model file: " << modelFilename;
                continue;
            }
        }
        std::string colorString;
        {
            // Load color
            std::string colorFilename = templatePath + "/" + objectName + ".color";
            std::ifstream input(colorFilename.c_str());
            if (!(input >> colorString))
            {
                ARMARX_WARNING << "Could not load color file: " << colorFilename;
                continue;
            }
            ObjectColor color = CColorParameterSet::Translate(colorString.c_str());
            if (color == eNone)
            {
                ARMARX_WARNING << "Unknown color string: " << colorString;
                continue;
            }
            entry.color = color;
        }
 
        ARMARX_INFO << "Inserting template into database: " << objectName
                    << ", template count: " << entry.data->templateCount
                    << ", color: " << colorString << " (" << entry.color << ")";
        database.emplace(objectName, std::move(entry));
    }
 
    if (!colorParameters.LoadFromFile(colorParemeterFilename.c_str()))
    {
        throw armarx::LocalException("Could not read color parameter file.");
    }
 
    // create context
    ImageFormatInfo imageFormat = getImageFormat();
    contextGL.reset(new CGLContext());
    contextGL->CreateContext(imageFormat.dimension.width, imageFormat.dimension.height);
    contextGL->MakeCurrent();
 
    m_pOpenGLVisualizer.reset(new COpenGLVisualizer());
    m_pOpenGLVisualizer->InitByCalibration(getStereoCalibration()->GetRightCalibration());
 
    m_pObjectFinderStereo.reset(new CObjectFinderStereo());
    m_pObjectFinderStereo->SetColorParameterSet(&colorParameters);
    m_pObjectFinderStereo->Init(getStereoCalibration());
 
    ARMARX_INFO << "SegmentableTemplateRecognition is initialized";
 
    return true;
}
 
bool
SegmentableTemplateRecognition::addObjectClass(const memoryx::EntityPtr& objectClassEntity,
                                               const memoryx::GridFileManagerPtr& fileManager)
{
    std::string entityName = objectClassEntity->getName();
    ARMARX_VERBOSE << "Adding object class " << objectClassEntity->getName() << armarx::flush;
 
    auto iter = database.find(entityName);
    return iter != database.end();
}
 
memoryx::ObjectLocalizationResultList
SegmentableTemplateRecognition::localizeObjectClasses(
    const std::vector<std::string>& objectClassNames,
    CByteImage** cameraImages,
    armarx::MetaInfoSizeBasePtr imageMetaInfo,
    CByteImage** resultImages)
{
 
    if (objectClassNames.size() < 1)
    {
        ARMARX_WARNING << "objectClassNames.size() = " << objectClassNames.size()
                       << ", something is wrong here! ";
 
        memoryx::ObjectLocalizationResultList resultList;
        return resultList;
    }
 
    ARMARX_VERBOSE << "Localizing " << objectClassNames;
 
    contextGL->MakeCurrent();
 
    Object3DList objectList;
 
    for (std::string const& className : objectClassNames)
    {
        Object3DList classObjectList;
 
        int dilationCount = 2;
 
        auto classEntryIter = database.find(className);
        if (classEntryIter == database.end())
        {
            ARMARX_WARNING << "Could not find database entry for: " << className;
            continue;
        }
        SegmentableTemplateEntry const& classEntry = classEntryIter->second;
        ObjectColor color = classEntry.color;
 
        std::vector<HSVSegmentationParams> hsvSegmentationParams;
        // FIXME: Hack to allow two colored objects
        if (color == eYellow3)
        {
            hsvSegmentationParams.push_back(getSegmentationParamsFromColor(colorParameters, eRed));
            hsvSegmentationParams.push_back(
                getSegmentationParamsFromColor(colorParameters, eYellow));
            hsvSegmentationParams.back().minRatio = 0.2f;
        }
        else
        {
            HSVSegmentationParams hsvSegmentationParam =
                getSegmentationParamsFromColor(colorParameters, color);
            hsvSegmentationParams.push_back(hsvSegmentationParam);
        }
 
        {
            CByteImage* leftInput = cameraImages[0];
            CByteImage* rightInput = cameraImages[1];
            int width = leftInput->width;
            int height = leftInput->height;
            ARMARX_CHECK_EXPRESSION(rightInput->width == width && rightInput->height == height);
 
            CByteImage grayImageBig(width, height, CByteImage::eGrayScale);
            ::ImageProcessor::ConvertImage(leftInput, &grayImageBig, true);
 
            BlobDetector leftBlobDetector(width, height);
            BlobDetector rightBlobDetector(width, height);
 
            Object2DList leftObjects = leftBlobDetector.findColoredRegions(
                leftInput, paramMinPixelsPerRegion, hsvSegmentationParams, dilationCount);
            Object2DList rightObjects = rightBlobDetector.findColoredRegions(
                rightInput, paramMinPixelsPerRegion, hsvSegmentationParams, dilationCount);
 
            for (Object2DEntry const& entry : leftObjects)
            {
                ::PrimitivesDrawer::DrawRegion(resultImages[0], entry.region, 255, 0, 0, 2);
            }
 
            // Calculate blobs with 3D pose from stereo calibration
            Object3DList blobList;
 
            float maxEpipolarDistance = this->maxEpipolarDistance;
            CStereoCalibration* stereoCalibration = getStereoCalibration();
            bool inputImagesAreRectified = false; // FIXME: Should this be true?
            bool useDistortionParameters = !getImagesAreUndistorted();
            float minZDistance = 500;
            float maxZDistance = 3000;
            {
                for (Object2DEntry& leftObject : leftObjects)
                {
                    Object2DEntry const* bestMatch = nullptr;
                    float bestDiff = maxEpipolarDistance;
                    for (Object2DEntry& rightObject : rightObjects)
                    {
                        float pixelRatio =
                            leftObject.region.nPixels < rightObject.region.nPixels
                                ? (float)leftObject.region.nPixels / rightObject.region.nPixels
                                : (float)rightObject.region.nPixels / leftObject.region.nPixels;
                        float aspectRatioRatio =
                            leftObject.region.ratio < rightObject.region.ratio
                                ? (float)leftObject.region.ratio / rightObject.region.ratio
                                : (float)rightObject.region.ratio / leftObject.region.ratio;
 
                        Vec2d leftCentroid = leftObject.region.centroid;
                        Vec2d rightCentroid = rightObject.region.centroid;
 
                        float yDiff;
                        if (inputImagesAreRectified)
                        {
                            yDiff = fabsf(leftCentroid.y - rightCentroid.y);
                        }
                        else
                        {
                            yDiff = stereoCalibration->CalculateEpipolarLineInLeftImageDistance(
                                leftCentroid, rightCentroid);
                            yDiff = fabs(yDiff);
                        }
 
                        Vec3d position;
                        stereoCalibration->Calculate3DPoint(leftCentroid,
                                                            rightCentroid,
                                                            position,
                                                            inputImagesAreRectified,
                                                            useDistortionParameters);
 
 
                        if (pixelRatio > 0.5f && aspectRatioRatio > 0.5f &&
                            yDiff <= maxEpipolarDistance && position.z >= minZDistance &&
                            position.z <= maxZDistance && yDiff <= bestDiff)
                        {
                            bestDiff = yDiff;
                            bestMatch = &rightObject;
                        }
                    }
 
                    // We now have the best matching right region in bestMatch
                    if (bestMatch)
                    {
                        Object2DEntry const& rightObject = *bestMatch;
                        Object3DEntry entry;
 
                        entry.region_left = leftObject.region;
                        entry.region_right = rightObject.region;
                        entry.region_id_left = leftObject.id;
                        entry.region_id_right = rightObject.id;
 
                        stereoCalibration->Calculate3DPoint(entry.region_left.centroid,
                                                            entry.region_right.centroid,
                                                            entry.pose.translation,
                                                            inputImagesAreRectified,
                                                            useDistortionParameters);
 
                        entry.world_point = entry.pose.translation;
 
                        blobList.push_back(entry);
                    }
                }
            }
 
            // Visualize results
            CByteImage* segmented = &leftBlobDetector.segmented;
            CByteImage* resultImage = resultImages[0];
            {
                const unsigned char* input = segmented->pixels;
                unsigned char* output = resultImage->pixels;
                int nPixels = segmented->width * segmented->height;
 
                // FIXME: Make the highlight color configurable (depending on query color?)
                int r, g, b;
                hsv2rgb(0, 255, 100, r, g, b);
 
                for (int i = 0; i < nPixels; i++)
                {
                    if (input[i])
                    {
                        const int offset = 3 * i;
                        output[offset + 0] = r;
                        output[offset + 1] = g;
                        output[offset + 2] = b;
                    }
                }
            }
 
 
            // Cut out segmented part of the image
            // segmentedObject contains the grayscale image of the blob (every other pixel is black)
            CByteImage segmentedObject(width, height, CByteImage::eGrayScale);
            ::ImageProcessor::ConvertImage(leftInput, &segmentedObject);
            ::ImageProcessor::And(segmented, &segmentedObject, &segmentedObject);
 
            // Try to match templates into the blobs
            ARMARX_VERBOSE << "Blob count: " << blobList.size();
            for (Object3DEntry& blob : blobList)
            {
                const MyRegion& region = blob.region_left;
 
                int length = SegmentableBitmapWidth;
                CByteImage objectGray(length, length, CByteImage::eGrayScale);
 
                //DEBUG_SAVE_IMAGE(*pGrayImage, "00");
 
                // Crop the region of the blob
                CropImageToTemplate(&segmentedObject, region, &objectGray);
 
                //DEBUG_SAVE_IMAGE(objectGray, "01");
 
                ::ImageProcessor::ThresholdBinarize(&objectGray, &objectGray, 1);
                //DEBUG_SAVE_IMAGE(objectGray, "03_binarize");
 
                // objectGray is now a black/white image (64x64) of the blob
                CByteImage* input = &objectGray;
                SegmentableTemplateHeader const& templateHeader = *classEntry.data;
 
                int bestOverlap = 0;
                int total = 0;
                SegmentableTemplate const* bestTemplate = nullptr;
                for (std::uint32_t i = 0; i < templateHeader.templateCount; ++i)
                {
                    SegmentableTemplate const& template_ = templateHeader.templates[i];
 
                    // Calculate the overlap between input and template_.bitmap
                    int overlap = 0;
                    int stride = input->width;
                    total = 0;
                    for (int y = 0; y < (int)SegmentableBitmapWidth; ++y)
                    {
                        std::uint32_t const* templateRow = template_.bitmap + y * 2;
                        unsigned char const* inputRow = input->pixels + y * stride;
                        for (int x = 0; x < (int)(SegmentableBitmapWidth / 32); ++x)
                        {
                            std::uint32_t compressed = templateRow[x];
                            // Decompress
                            for (int i = 0; i < 32; ++i)
                            {
                                bool inputPixel = inputRow[x * 32 + i] > 0;
                                bool templatePixel = (compressed & (1 << i)) > 0;
                                if ((inputPixel && templatePixel) ||
                                    (!inputPixel && !templatePixel))
                                {
                                    ++overlap;
                                }
                                ++total;
                            }
                        }
                    }
 
                    if (overlap > bestOverlap)
                    {
                        bestOverlap = overlap;
                        bestTemplate = &template_;
                    }
                }
 
                float overlapRatio =
                    1.0f * bestOverlap / (SegmentableBitmapWidth * SegmentableBitmapWidth);
 
                if (!bestTemplate || overlapRatio < paramTemplateMatchThreshold)
                {
                    ARMARX_VERBOSE
                        << "Could not find a matching template (overlap ratio: " << overlapRatio
                        << ")";
                }
                else
                {
                    // Calculate pose from angles
                    {
                        Transformation3d& poseData = blob.pose;
                        Vec3d convention;
                        convention.x = bestTemplate->rotationX;
                        convention.y = bestTemplate->rotationY;
                        convention.z = bestTemplate->rotationZ;
 
                        // calculate rotation matrix
                        Mat3d temp;
                        Math3d::SetRotationMatX(poseData.rotation, convention.x);
                        Math3d::SetRotationMatZ(temp, convention.z);
                        Math3d::MulMatMat(poseData.rotation, temp, poseData.rotation);
                        Math3d::SetRotationMatY(temp, convention.y);
                        Math3d::MulMatMat(poseData.rotation, temp, poseData.rotation);
                    }
 
                    // Calculate corrected orientation
                    Vec3d position = blob.pose.translation;
                    Mat3d orientation = blob.pose.rotation;
                    Mat3d& resultOrientation = blob.pose.rotation;
                    {
                        Vec3d u0 = {0, 0, 1};
                        Vec3d u = position;
                        Math3d::NormalizeVec(u);
 
                        // corrective rotation matrix
                        Mat3d correctiveRotation;
                        Vec3d axis;
                        Math3d::CrossProduct(u0, u, axis);
                        float angle = Math3d::Angle(u0, u, axis);
                        Math3d::SetRotationMatAxis(correctiveRotation, axis, angle);
 
                        // final rotation matrix
                        Math3d::MulMatMat(correctiveRotation, orientation, resultOrientation);
                    }
 
                    // Calculate corrected position
                    orientation = resultOrientation;
                    float resultCorrelation = 0.0f;
                    float resultSizeRatio = 0.0f;
                    CByteImage* initialMask = &objectGray;
                    int nSize = region.nPixels;
                    int nSimulatedSize = 0;
                    {
                        Vec3d initialPosition = position;
                        // Calculate position in rendered images
                        // Add difference pos_real - pos_sim to pos_real
                        // This results in the simulation producing the expected image
 
                        CStereoCalibration* stereoCalibration = getStereoCalibration();
                        const int width = stereoCalibration->width;
                        const int height = stereoCalibration->height;
 
                        Transformation3d pose;
                        Math3d::SetVec(pose.translation, position);
                        Math3d::SetMat(pose.rotation, orientation);
 
                        CByteImage image_left(width, height, CByteImage::eGrayScale);
                        CByteImage image_right(width, height, CByteImage::eGrayScale);
                        CByteImage* ppImages[2] = {&image_left, &image_right};
 
                        // render left image
                        m_pOpenGLVisualizer->Clear();
                        m_pOpenGLVisualizer->ActivateShading(false);
                        m_pOpenGLVisualizer->SetProjectionMatrix(
                            stereoCalibration->GetLeftCalibration());
                        //DrawObjectFromFile(m_pOpenGLVisualizer, entry->sOivPath, pose, entry->sName);
                        CFloatMatrix* model = database[className].model.get();
                        m_pOpenGLVisualizer->DrawObject(model, pose);
                        m_pOpenGLVisualizer->GetImage(&image_left);
                        ::ImageProcessor::FlipY(&image_left, &image_left);
 
                        // render right image
                        m_pOpenGLVisualizer->Clear();
                        m_pOpenGLVisualizer->SetProjectionMatrix(
                            stereoCalibration->GetRightCalibration());
                        //DrawObjectFromFile(m_pOpenGLVisualizer, entry->sOivPath, pose, entry->sName);
                        m_pOpenGLVisualizer->DrawObject(model, pose);
                        m_pOpenGLVisualizer->GetImage(&image_right);
                        ::ImageProcessor::FlipY(&image_right, &image_right);
 
                        //                                DEBUG_SAVE_IMAGE(image_left, "04_simulated");
                        //                                DEBUG_SAVE_IMAGE(image_right, "05_simulated");
 
                        // calculate stereo
                        m_pObjectFinderStereo->ClearObjectList();
                        m_pObjectFinderStereo->FindObjectsInSegmentedImage(
                            ppImages, 0, eRed, 100, false);
                        m_pObjectFinderStereo->Finalize(0, 10000, false, eRed, 10, false);
                        const Object3DList& objectList = m_pObjectFinderStereo->GetObject3DList();
 
                        if (objectList.size() == 1)
                        {
                            Object3DEntry const& simEntry = objectList[0];
 
                            Vec3d positionCorrection;
                            Math3d::SubtractVecVec(
                                position, simEntry.pose.translation, positionCorrection);
                            Math3d::AddVecVec(initialPosition, positionCorrection, position);
 
                            // calculate quality measure
                            // get simulated size
                            nSimulatedSize = objectList.at(0).region_left.nPixels;
 
                            // calculate size normalized image
                            const int k = SegmentableBitmapWidth;
                            CByteImage simulatedObject(k, k, CByteImage::eGrayScale);
                            CropImageToTemplate(
                                &image_left, simEntry.region_left, &simulatedObject);
 
                            int sum = 0;
                            for (std::size_t j = 0; j < SegmentableBitmapSize; j++)
                            {
                                sum += abs(simulatedObject.pixels[j] - initialMask->pixels[j]);
                            }
 
                            // calculate final measures
                            float divisor = 255.0f * SegmentableBitmapSize;
                            resultCorrelation = 1.0f - sum / divisor;
                            resultSizeRatio = nSimulatedSize < nSize
                                                  ? float(nSimulatedSize) / nSize
                                                  : float(nSize) / nSimulatedSize;
                        }
                        else
                        {
                            ARMARX_VERBOSE << "Could not find object '" << className
                                           << "' in simulated image";
                        }
                    }
 
                    if (resultSizeRatio < paramSizeRatioThreshold)
                    {
                        ARMARX_VERBOSE << "Detected blob size is too different in real image "
                                          "compared to simulated image ("
                                       << "real size: " << nSize
                                       << ", simulated size: " << nSimulatedSize
                                       << ", ratio: " << resultSizeRatio << ")";
                    }
                    else if (resultCorrelation < paramCorrelationThreshold)
                    {
                        ARMARX_VERBOSE
                            << "Detected blob does not correlate with the simulated blob ("
                            << "ratio: " << resultCorrelation << ")";
                    }
                    else
                    {
                        blob.sName = className;
                        blob.class_id = 0;
                        blob.quality = resultSizeRatio; //fResultRatio;
                        blob.quality2 = resultCorrelation; //fResultCorrelation;
                        blob.localizationValid = true;
 
                        ARMARX_VERBOSE << "Found a match (overlap ratio: " << overlapRatio
                                       << ", size ratio: " << resultSizeRatio
                                       << ", correlation: " << resultCorrelation
                                       << ", blob size: " << nSize << ")";
 
                        classObjectList.push_back(blob);
                    }
                }
            }
            if (classObjectList.size() > 1 && paramSingleInstance)
            {
                std::sort(classObjectList.begin(),
                          classObjectList.end(),
                          [](Object3DEntry const& left, Object3DEntry const& right)
                          { return left.quality > right.quality; });
                Object3DEntry& bestMatch = classObjectList.front();
                objectList.push_back(bestMatch);
            }
            else
            {
                objectList.insert(objectList.end(), classObjectList.begin(), classObjectList.end());
            }
        }
    }
 
    ARMARX_VERBOSE << "Found " << objectList.size() << " object(s)";
 
    // help for problem analysis
    if (objectList.size() == 0)
    {
        ARMARX_INFO << "Looked unsuccessfully for:";
        std::string color;
 
        for (std::string const& className : objectClassNames)
        {
            CColorParameterSet::Translate(database[className].color, color);
            ARMARX_INFO << className << " color: " << color << flush;
        }
    }
    else
    {
        visualizeResults(objectList, resultImages);
    }
 
    const auto agentName = getProperty<std::string>("AgentName").getValue();
 
    memoryx::ObjectLocalizationResultList resultList;
 
    for (Object3DList::iterator iter = objectList.begin(); iter != objectList.end(); iter++)
    {
        float x = iter->pose.translation.x;
        float y = iter->pose.translation.y;
        float z = iter->pose.translation.z;
 
        if (seq.count(iter->sName))
        {
            seq[iter->sName]++;
        }
        else
        {
            seq[iter->sName] = 0;
        }
 
        StringVariantBaseMap mapValues;
        mapValues["x"] = new Variant(x);
        mapValues["y"] = new Variant(y);
        mapValues["z"] = new Variant(z);
        mapValues["name"] = new Variant(iter->sName);
        mapValues["sequence"] = new Variant(seq[iter->sName]);
        mapValues["timestamp"] = new Variant(imageMetaInfo->timeProvided / 1000.0 / 1000.0);
        debugObserver->setDebugChannel("ObjectRecognition", mapValues);
 
        // only accept realistic positions
        if (x > validResultBoundingBoxMin.x && y > validResultBoundingBoxMin.y &&
            z > validResultBoundingBoxMin.z && x < validResultBoundingBoxMax.x &&
            y < validResultBoundingBoxMax.y && z < validResultBoundingBoxMax.z)
        {
            // assemble result
            memoryx::ObjectLocalizationResult result;
 
            // position and orientation
            Eigen::Vector3f position(
                iter->pose.translation.x, iter->pose.translation.y, iter->pose.translation.z);
            Eigen::Matrix3f orientation;
            orientation << iter->pose.rotation.r1, iter->pose.rotation.r2, iter->pose.rotation.r3,
                iter->pose.rotation.r4, iter->pose.rotation.r5, iter->pose.rotation.r6,
                iter->pose.rotation.r7, iter->pose.rotation.r8, iter->pose.rotation.r9;
 
            ARMARX_VERBOSE << "Reporting in frame: " << referenceFrameName;
            result.position = new armarx::FramedPosition(position, referenceFrameName, agentName);
            result.orientation =
                new armarx::FramedOrientation(orientation, referenceFrameName, agentName);
 
            // calculate noise
            result.positionNoise = calculateLocalizationUncertainty(iter->region_left.centroid,
                                                                    iter->region_right.centroid);
 
            // calculate recognition certainty
            result.recognitionCertainty =
                0.5f + 0.5f * calculateRecognitionCertainty(iter->sName, *iter);
            result.objectClassName = iter->sName;
            result.timeStamp = new TimestampVariant(imageMetaInfo->timeProvided);
 
            resultList.push_back(result);
        }
        else
        {
            ARMARX_VERBOSE << "Refused unrealistic localization at position: " << x << " " << y
                           << " " << z;
        }
    }
 
    ARMARX_VERBOSE << "Finished localizing " << objectClassNames.at(0);
 
    return resultList;
}
 
float
SegmentableTemplateRecognition::calculateRecognitionCertainty(const std::string& objectClassName,
                                                              const Object3DEntry& entry)
{
    float foundProb = entry.quality * entry.quality2;
    float notFoundProb = (1 - entry.quality) * (1 - entry.quality2);
 
    if (foundProb <= 0)
    {
        return 0.0f;
    }
 
    return foundProb / (foundProb + notFoundProb);
}
 
void
SegmentableTemplateRecognition::visualizeResults(const Object3DList& objectList,
                                                 CByteImage** resultImages)
{
    m_pOpenGLVisualizer->ActivateShading(true);
    glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
 
 
    m_pOpenGLVisualizer->SetProjectionMatrix(getStereoCalibration()->GetRightCalibration());
 
    m_pOpenGLVisualizer->Clear();
 
 
    for (int i = 0; i < (int)objectList.size(); i++)
    {
        const Object3DEntry& entry = objectList.at(i);
        CFloatMatrix* model = database[entry.sName].model.get();
        m_pOpenGLVisualizer->DrawObject(model, entry.pose);
    }
 
    const int nImageIndex = 1;
 
    if (resultImages && resultImages[nImageIndex])
    {
        CByteImage tempImage(resultImages[nImageIndex]);
        m_pOpenGLVisualizer->GetImage(&tempImage);
        ::ImageProcessor::FlipY(&tempImage, &tempImage);
        const int nBytes = 3 * tempImage.width * tempImage.height;
        const unsigned char* pixels = tempImage.pixels;
        unsigned char* output = resultImages[nImageIndex]->pixels;
 
        for (int i = 0; i < nBytes; i += 3)
        {
            if (pixels[i])
            {
                const unsigned char g = pixels[i];
                output[i] = g;
                output[i + 1] = g;
                output[i + 2] = g;
            }
        }
    }
}