FaceRecognition.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    VisionX::ArmarXObjects::FaceRecognition
 * @author     Markus Grotz ( markus dot grotz at kit dot edu )
 * @date       2016
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "FaceRecognition.h"
#include <Image/PrimitivesDrawerCV.h>
#include <Image/PrimitivesDrawer.h>
#include <VisionX/tools/TypeMapping.h>
 
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
 
#include <Image/ImageProcessor.h>
#include <Image/IplImageAdaptor.h>
 
#include <Eigen/Geometry>
 
using namespace armarx;
 
 
void armarx::FaceRecognition::onInitObjectLocalizerProcessor()
{
 
    stereoMatcher = new CStereoMatcher();
 
    model = cv::face::FisherFaceRecognizer(0, 1000.0);
 
    armarx::CMakePackageFinder finder("VisionX");
    ArmarXDataPath::addDataPaths(finder.getDataDir());
 
    std::string trainingDataPath = getProperty<std::string>("trainingDataPath").getValue();
 
    if (!ArmarXDataPath::getAbsolutePath(trainingDataPath, trainingDataPath))
    {
        ARMARX_ERROR << "Could not find data file in ArmarXDataPath: " << trainingDataPath;
    }
 
    std::string classifierFileName = getProperty<std::string>("classifierFileName").getValue();
 
    if (!ArmarXDataPath::getAbsolutePath(classifierFileName, classifierFileName))
    {
        ARMARX_ERROR << "Could not find data file in ArmarXDataPath: " << classifierFileName;
    }
 
    if (!std::filesystem::is_directory(trainingDataPath))
    {
        ARMARX_FATAL << "unable to load training model";
 
    }
 
    std::vector<std::string> fileNames;
 
    for (auto iter = std::filesystem::directory_iterator(trainingDataPath);
         iter != std::filesystem::directory_iterator(); ++iter)
    {
        fileNames.push_back(iter->path().string());
    }
 
    std::sort(fileNames.begin(),  fileNames.end());
 
    std::string currentLabel;
 
    std::vector<cv::Mat> images;
    std::vector<int> index;
 
    for (std::string& fileName : fileNames)
    {
        const auto stem = std::filesystem::path(fileName).stem().string();
        std::vector<std::string> strs = simox::alg::split(stem, "_");
        std::string label = strs[0];
 
        if (currentLabel != label)
        {
            labels[labels.size()] = label;
            currentLabel = label;
        }
        images.push_back(cv::imread(fileName, CV_LOAD_IMAGE_GRAYSCALE));
        index.push_back(labels.size() - 1);
    }
 
    ARMARX_LOG << "total number of classes: " << labels.size();
 
    model->train(images, index);
 
    faceImageSize = images[0].size();
 
    classifier.load(classifierFileName);
 
 
}
 
void armarx::FaceRecognition::onConnectObjectLocalizerProcessor()
{
    stereoMatcher->InitCameraParameters(getStereoCalibration(), false);
}
 
void armarx::FaceRecognition::onExitObjectLocalizerProcessor()
{
 
    delete stereoMatcher;
}
 
bool armarx::FaceRecognition::initRecognizer()
{
    return true;
}
 
bool armarx::FaceRecognition::addObjectClass(const memoryx::EntityPtr& objectClassEntity, const memoryx::GridFileManagerPtr& fileManager)
{
    ///@TODO
    throw std::logic_error {"bool armarx::FaceRecognition::addObjectClass not implemented yet"};
}
 
 
memoryx::ObjectLocalizationResultList armarx::FaceRecognition::localizeObjectClasses(const std::vector<std::string>& objectClassNames, CByteImage** cameraImages, armarx::MetaInfoSizeBasePtr imageMetaInfo, CByteImage** resultImages)
{
 
    memoryx::ObjectLocalizationResultList resultList;
    cv::Mat result = cv::cvarrToMat(IplImageAdaptor::Adapt(resultImages[0]));
 
 
    const cv::Mat tempRGBImage = cv::cvarrToMat(IplImageAdaptor::Adapt(cameraImages[0]));
    cv::Mat original;
    cv::cvtColor(tempRGBImage, original, CV_RGB2BGR);
 
    cv::Mat gray;
    cv::cvtColor(original, gray, CV_BGR2GRAY);
 
    std::vector<cv::Rect_<int>> faces;
    classifier.detectMultiScale(gray, faces, 1.05, 2);
 
    const std::string refFrame = getProperty<std::string>("ReferenceFrameName").getValue();
    const std::string agentName = getProperty<std::string>("AgentName").getValue();
 
    ARMARX_INFO << deactivateSpam(5, std::to_string(faces.size())) << "found " << faces.size() << " possible faces";
    for (cv::Rect rect : faces)
    {
        cv::Mat face = gray(rect);
 
        cv::Mat faceResized;
        cv::resize(face, faceResized, faceImageSize, 1.0, 1.0, cv::INTER_CUBIC);
 
        int predictedLabel = -1;
        double predictedConfidence = 0.0;
        model->predict(faceResized, predictedLabel, predictedConfidence);
 
        std::string temp = labels[predictedLabel] + "=%0.1f";
        std::string label = cv::format(temp.c_str(), predictedConfidence);
 
 
        if (predictedLabel < 0)
        {
            cv::rectangle(result, rect, CV_RGB(0, 0, 255), 1);
        }
        else
        {
            cv::rectangle(result, rect, CV_RGB(0, 255, 0), 1);
 
            int posX = std::max(rect.tl().x - 10, 0);
            int posY = std::max(rect.tl().y - 10, 0);
            putText(result, label, cv::Point(posX, posY), cv::FONT_HERSHEY_PLAIN, 1.0, CV_RGB(0, 255, 0), 2.0);
 
            if (std::find(objectClassNames.begin(), objectClassNames.end(), label) != objectClassNames.end())
            {
                // todo see ObjectLearningByPushing/FeatureCalculation.cpp
                CByteImage* imgRightGray = new CByteImage(cameraImages[1]->width, cameraImages[1]->height, CByteImage::eGrayScale);
                CByteImage* imgLeftGray = new CByteImage(cameraImages[1]->width, cameraImages[1]->height, CByteImage::eGrayScale);
                ::ImageProcessor::ConvertImage(cameraImages[0], imgLeftGray);
                ::ImageProcessor::ConvertImage(cameraImages[1], imgRightGray);
 
                Vec2d vCorrespondingPointRight;
                Vec3d vPoint3D;
                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) rect.x + rect.width / 2.0, (int) rect.y + rect.height / 2.0,
                                      std::max(rect.width, rect.height), nDispMin, nDispMax, vCorrespondingPointRight, vPoint3D, 0.7f, true);
 
 
                delete imgRightGray;
                delete imgLeftGray;
 
                if (nMatchingResult >= 0)
                {
                    Eigen::Vector3f position(vPoint3D.x, vPoint3D.y, vPoint3D.z);
                    Eigen::Matrix3f orientation = Eigen::Matrix3f::Identity() * Eigen::AngleAxisf(-M_PI, Eigen::Vector3f::UnitZ());
 
                    memoryx::ObjectLocalizationResult result;
 
                    result.position = new armarx::FramedPosition(position, refFrame, agentName);
                    result.orientation = new armarx::FramedOrientation(orientation, refFrame, agentName);
                    result.recognitionCertainty = 1.0 / predictedConfidence;
                    result.positionNoise = calculateLocalizationUncertainty(position);
                    result.objectClassName = label;
 
                    resultList.push_back(result);
                }
            }
        }
    }
 
    return resultList;
}
 
 
 
armarx::PropertyDefinitionsPtr FaceRecognition::createPropertyDefinitions()
{
    return armarx::PropertyDefinitionsPtr(new FaceRecognitionPropertyDefinitions(
            getConfigIdentifier()));
}