DeepFaceRecognition.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::DeepFaceRecognition
 * @author     Mirko Waechter (waechter at kit dot edu)
 * @date       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "DeepFaceRecognition.h"
 
#include <Eigen/Geometry>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
 
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include <VisionX/tools/TypeMapping.h>
 
#include <Calibration/Calibration.h>
#include <Image/ImageProcessor.h>
#include <Image/IplImageAdaptor.h>
#include <Image/PrimitivesDrawer.h>
#include <Image/PrimitivesDrawerCV.h>
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/core/memory/PersistentEntitySegment.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
 
using namespace armarx;
using namespace visionx;
 
//void visionx::DeepFaceRecognition::onInitObjectLocalizerProcessor()
void
visionx::DeepFaceRecognition::onInitComponent()
{
    usingProxy("FaceRecognition");
    usingProxy("LongtermMemory");
    usingProxy(getProperty<std::string>("ImageProviderName").getValue());
 
 
    offeringTopic("TextToSpeech");
}
 
//void visionx::DeepFaceRecognition::onConnectObjectLocalizerProcessor()
void
visionx::DeepFaceRecognition::onConnectComponent()
{
    getProxy(faceReg, "FaceRecognition");
    getProxy(ltm, "LongtermMemory");
    getProxy(imageProvider, getProperty<std::string>("ImageProviderName").getValue());
    //    auto calibration = imageProvider->getMonocularCalibration();
 
    //    this->calibration.reset(visionx::tools::convert(imageProvider->getMonocularCalibration()));
    this->calibration.reset(new CCalibration());
    calibration->SetCameraParameters(
        987.99, 956.82, 639.32, 415.64, 0, 0, 0, 0, Math3d::unit_mat, Math3d::zero_vec, 1280, 720);
 
    tts = getTopic<armarx::TextListenerInterfacePrx>("TextToSpeech");
    if (!ltm->hasSegment(faceSegmentName))
    {
        faceSegmentPrx = memoryx::EntityMemorySegmentInterfacePrx::checkedCast(
            ltm->addGenericSegment(faceSegmentName));
    }
    else
    {
        faceSegmentPrx =
            memoryx::EntityMemorySegmentInterfacePrx::checkedCast(ltm->getSegment(faceSegmentName));
    }
}
 
armarx::PropertyDefinitionsPtr
DeepFaceRecognition::createPropertyDefinitions()
{
    return armarx::PropertyDefinitionsPtr(
        new DeepFaceRecognitionPropertyDefinitions(getConfigIdentifier()));
}
 
memoryx::ObjectLocalizationResultList
visionx::DeepFaceRecognition::localizeObjectClasses(const memoryx::ObjectClassNameList& classes,
                                                    const Ice::Current&)
{
    ARMARX_VERBOSE << deactivateSpam(10) << "Localizing: " << classes;
    memoryx::ObjectLocalizationResultList result;
    ARMARX_CHECK_EXPRESSION(faceReg);
    ARMARX_CHECK_EXPRESSION(faceSegmentPrx);
 
    auto faceLocations = faceReg->calculateFaceLocations();
    ARMARX_VERBOSE << deactivateSpam(10, std::to_string(faceLocations.size())) << "Found  "
                   << faceLocations.size() << " faces";
    auto agentName = getProperty<std::string>("AgentName").getValue();
    auto cameraFrame = getProperty<std::string>("CameraFrameName").getValue();
    for (visionx::FaceLocation faceLocation : faceLocations)
    {
        memoryx::EntityPtr oldFace =
            memoryx::EntityPtr::dynamicCast(faceSegmentPrx->getEntityByName(faceLocation.label));
        bool updateFaceMemory = true;
        ARMARX_DEBUG << deactivateSpam(1) << "Rect: " << faceLocation.topLeft.e0 << ", "
                     << faceLocation.topLeft.e1 << ", " << faceLocation.bottomRight.e0 << ", "
                     << faceLocation.bottomRight.e1;
        float centerX = (faceLocation.bottomRight.e0 + faceLocation.topLeft.e0) * 0.5f;
        float centerY = (faceLocation.bottomRight.e1 + faceLocation.topLeft.e1) * 0.5f;
        memoryx::ObjectLocalizationResult r;
        r.objectClassName = "face";
        r.instanceName = faceLocation.label;
        Eigen::Matrix3f ori = Eigen::Matrix3f::Identity();
        r.orientation = new FramedOrientation(ori, cameraFrame, agentName);
        Vec3d cameraCoords;
        float faceHeightPixel = faceLocation.bottomRight.e1 - faceLocation.topLeft.e1;
        float distance =
            calibration->GetCameraParameters().focalLength.y * 160.f / (faceHeightPixel);
        this->calibration->ImageToCameraCoordinates(
            Vec2d{centerX, centerY}, cameraCoords, distance, false);
        ARMARX_VERBOSE << deactivateSpam(1) << "Camera coordinates for " << faceLocation.label
                       << ": " << cameraCoords.x << ", " << cameraCoords.y
                       << " distance: " << distance << ", " << VAROUT(faceHeightPixel)
                       << VAROUT(centerX) << VAROUT(centerY);
        r.position = new FramedPosition(
            Eigen::Vector3f(cameraCoords.x, cameraCoords.y, distance), cameraFrame, agentName);
        Eigen::Matrix3f id = Eigen::Matrix3f::Identity();
        r.orientation = new FramedOrientation(id, cameraFrame, agentName);
        r.recognitionCertainty = 0.7f;
        Eigen::Vector3f cov(10, 10, 10000);
        r.positionNoise =
            new memoryx::MultivariateNormalDistribution(Eigen::Vector3f::Zero(), cov.asDiagonal());
 
        r.timeStamp = new armarx::TimestampVariant(faceLocation.timestamp);
        result.push_back(r);
        if (faceLocation.label == "Unknown")
        {
            continue;
        }
        if (oldFace && oldFace->hasAttribute("LastGreetingTime"))
        {
            VariantPtr updateTimeVariant =
                VariantPtr::dynamicCast(oldFace->getAttribute("LastGreetingTime")->getValue());
            ARMARX_CHECK_EXPRESSION(updateTimeVariant);
            ARMARX_CHECK_EXPRESSION(updateTimeVariant->get<TimestampVariant>());
            updateFaceMemory &=
                updateTimeVariant->get<TimestampVariant>()->toTime() +
                    IceUtil::Time::secondsDouble(getProperty<float>("GreetAgainDelay").getValue()) <
                TimeUtil::GetTime();
        }
        auto face = oldFace ? oldFace : memoryx::Entity::CreateGenericEntity();
        ARMARX_CHECK_EXPRESSION(face);
        //        int matchCounter = 0;
        if (updateFaceMemory)
        {
            //            if (oldFace && oldFace->hasAttribute("MatchCounter"))
            //            {
            //                bool lastLocalizationTooOld  = false;
            //                if (oldFace->hasAttribute("UpdateTime"))
            //                {
            //                    VariantPtr updateTimeVariant = VariantPtr::dynamicCast(oldFace->getAttribute("UpdateTime")->getValue());
            //                    lastLocalizationTooOld &= updateTimeVariant->get<TimestampVariant>()->toTime() + IceUtil::Time::secondsDouble(10) > TimeUtil::GetTime();
 
            //                }
            //                if (!lastLocalizationTooOld)
            //                {
            //                    matchCounter = oldFace->getAttribute("MatchCounter")->getValue()->getInt();
            //                }
            //            }
            //            matchCounter++;
            face->setName(faceLocation.label);
 
            face->putAttribute("ImagePosition", new armarx::Vector2(centerX, centerY));
            face->putAttribute("UpdateTime", TimestampVariant::nowPtr());
            //            if (matchCounter >= getProperty<int>("FaceMatchCounter").getValue())
            float prob = updateAndCheckFaceExistenceProbability(faceLocation);
            ARMARX_INFO << deactivateSpam(1) << VAROUT(prob);
            if (prob > getProperty<float>("GreetThreshold").getValue())
 
            {
                auto tts_label = simox::alg::replace_all(faceLocation.label, "mirko", "mir ko");
                face->putAttribute("LastGreetingTime", TimestampVariant::nowPtr());
                tts->reportTextWithParams("Hello %1%.", {tts_label});
                ARMARX_IMPORTANT << "Hello " << faceLocation.label;
                //                matchCounter = 0; // reset back to 0 so
            }
        }
        //        face->putAttribute("MatchCounter", matchCounter); //always update matchCounter, though matchCounter is only increased if LastGreetingTime is old (see GreetAgainDelay)
        faceSegmentPrx->upsertEntityByName(faceLocation.label, face);
    }
    return result;
}
 
float
visionx::DeepFaceRecognition::updateAndCheckFaceExistenceProbability(
    const visionx::FaceLocation& faceLocation)
{
    auto& timeConfList = faceConfidenceHistory[faceLocation.label];
 
    double filterRadius = getProperty<float>("FilterRadius").getValue(); // kind of seconds
    double sigma = filterRadius / 2.5;
    double sigmaSquare = sigma * sigma;
    auto gaussianBellCurve = [&](double x) { return exp(-double((x) * (x)) / (sigmaSquare)); };
 
    auto now = IceUtil::Time::now();
    // remove old entries
    for (auto it = timeConfList.begin(); it != timeConfList.end();)
    {
        IceUtil::Time t = it->first;
        //        double confidence = it->second;
        double tDiff = (t - now).toMilliSecondsDouble() * 0.001;
        double weight = gaussianBellCurve(tDiff);
        //        ARMARX_INFO << VAROUT(tDiff) << VAROUT(weight) << t.toDateTime() + " " << now.toDateTime();
        if (weight < 0.001)
        {
            it = timeConfList.erase(it);
        }
        else
        {
            it++;
        }
    }
 
    timeConfList.push_back(std::make_pair(now, faceLocation.confidence));
    float sum = 0;
    for (auto& timeConfPair : timeConfList)
    {
        IceUtil::Time t = timeConfPair.first;
        double confidence = timeConfPair.second;
        double tDiff = (t - now).toMilliSecondsDouble() * 0.001;
        double weight = gaussianBellCurve(tDiff);
        //        ARMARX_INFO << VAROUT(confidence) << VAROUT(weight) << VAROUT(tDiff);
        sum += confidence * weight;
    }
    ARMARX_INFO << VAROUT(sum);
    return sum;
}