KalmanFilterFusion.cpp

Go to the documentation of this file.

#include "KalmanFilterFusion.h"
 
#include <MemoryX/libraries/helpers/EarlyVisionHelpers/EarlyVisionConverters.h>
#include <MemoryX/libraries/helpers/EarlyVisionHelpers/NoMotionFilter.h>
#include <MemoryX/libraries/motionmodels/AbstractMotionModel.h>
 
#include <MemoryX/libraries/helpers/EarlyVisionHelpers/EarlyVisionConverters.h>
#include <MemoryX/libraries/helpers/EarlyVisionHelpers/NoMotionFilter.h>
 
#include <RobotAPI/libraries/core/LinkedPose.h>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
 
namespace memoryx
{
 
    KalmanFilterFusion::KalmanFilterFusion() :
        EntityFusionMethod("KalmanFilterFusion")
    {
    }
 
    EntityBasePtr KalmanFilterFusion::initEntity(const EntityBasePtr& updateEntity, const Ice::Current&)
    {
        return updateEntity;
    }
 
    EntityBasePtr KalmanFilterFusion::fuseEntity(const EntityBasePtr& oldEntity, const EntityBasePtr& newEntity, const Ice::Current&)
    {
        ARMARX_DEBUG_S << "KalmanFilterFusion::fuseEntity() called";
 
        ObjectInstancePtr oldInstance = ObjectInstancePtr::dynamicCast(oldEntity);
        ObjectInstancePtr newInstance = ObjectInstancePtr::dynamicCast(newEntity);
        ObjectInstancePtr fusedInstance;
 
        ARMARX_CHECK_EXPRESSION(oldInstance);
        ARMARX_CHECK_EXPRESSION(newInstance);
 
        if (!oldInstance->getMotionModel())
        {
            return newInstance;
        }
 
        armarx::LinkedPoseBasePtr oldPoseBase = oldInstance->getMotionModel()->getPredictedPoseAtStartOfCurrentLocalization();
 
        if (oldPoseBase)
        {
            armarx::LinkedPosePtr oldPose = armarx::LinkedPosePtr::dynamicCast(oldPoseBase->clone());
            ARMARX_CHECK_EXPRESSION(oldPose);
            ARMARX_DEBUG_S << "Pose before fusion " << oldPose->output();
            ARMARX_DEBUG_S << "Pose of update " << newInstance->getPose()->output();
 
            oldPose->changeFrame(newInstance->getPosition()->frame);
 
 
 
            // make sure nothing is lost. TODO: clone new instance here instead of old one? -> Does not seem to work with newInstance->clone(); Then multiple instances appear
            fusedInstance = oldInstance->clone();
 
 
            // update existence certainty
 
            float oldExistenceCertainty = std::max(oldInstance->getExistenceCertainty(), 0.1f);
            float newExistenceCertainty = std::max(newInstance->getExistenceCertainty(), 0.1f);
            float fusedExistenceCertainty = (oldExistenceCertainty * newExistenceCertainty) / ((oldExistenceCertainty * newExistenceCertainty) + (1 - oldExistenceCertainty) * (1 - newExistenceCertainty));
            fusedInstance->setExistenceCertainty(fusedExistenceCertainty);
 
 
            // update position and orientation
 
            // setup filter
            NoMotionFilter filter;
            Eigen::MatrixXd i(3, 3);
            i.setIdentity();
 
            MultivariateNormalDistributionBasePtr newUncertainty = newInstance->getPositionUncertainty();
            MultivariateNormalDistributionBasePtr oldUncertainty = oldInstance->getPositionUncertainty();
 
            if (!newUncertainty)
            {
                ARMARX_ERROR_S << "new instance's position uncertainty is zero! - instance name: " << newInstance->getName();
                return newInstance->clone();
            }
 
            if (!oldUncertainty)
            {
                ARMARX_ERROR_S << "old instance's position uncertainty is zero! - instance name: " << oldInstance->getName();
                return newInstance->clone();
            }
 
            Gaussian measurement = EarlyVisionConverters::convertToGaussian(newUncertainty);
            filter.setMeasurementModel(i, measurement);
            Gaussian believe = EarlyVisionConverters::convertToGaussian(oldUncertainty);
            Gaussian posterior = filter.update(believe, measurement.getMean());
 
            // as the kalman filter position update canhave weird effects when the error is not actually coming from a normal distribution,
            // we do a simple linear interpolation instead
            const float meanRadiusOfMeasurementEllipsoid = pow(measurement.getCovariance().determinant(), 0.5 / measurement.getDimensions());
            const float meanRadiusOfOldBelieveEllipsoid = pow(believe.getCovariance().determinant(), 0.5 / believe.getDimensions());
            ARMARX_DEBUG_S << "Std dev of old believe: " << meanRadiusOfOldBelieveEllipsoid << ", std dev of measurement: " << meanRadiusOfMeasurementEllipsoid;
            const float interpolationFactor = 0.01f + 0.99f * meanRadiusOfOldBelieveEllipsoid / (meanRadiusOfMeasurementEllipsoid + meanRadiusOfOldBelieveEllipsoid);
            Eigen::Vector3f fusedPosition = interpolationFactor * newInstance->getPosition()->toEigen() + (1.0f - interpolationFactor) * oldPose->toEigen().block<3, 1>(0, 3);
            armarx::FramedPositionPtr pos = new armarx::FramedPosition(fusedPosition, oldPose->frame, oldPose->agent);
            fusedInstance->setPosition(pos);
 
            Eigen::Quaternionf measurementOrientation(newInstance->getOrientation()->qw, newInstance->getOrientation()->qx, newInstance->getOrientation()->qy, newInstance->getOrientation()->qz);
            Eigen::Quaternionf oldBelieveOrientation(oldPose->orientation->qw, oldPose->orientation->qx, oldPose->orientation->qy, oldPose->orientation->qz);
            Eigen::Quaternionf fusedOrientation = oldBelieveOrientation.slerp(interpolationFactor, measurementOrientation);
            fusedOrientation.normalize();
            armarx::FramedOrientationPtr ori = new armarx::FramedOrientation(fusedOrientation.toRotationMatrix(), oldPose->frame, oldPose->agent);
            fusedInstance->setOrientation(ori);
 
            // set position uncertainty of fused entity
            fusedInstance->setPositionUncertainty(EarlyVisionConverters::convertToMemoryX_MULTI(posterior));
 
            ARMARX_DEBUG_S << "Position after fusion: " << fusedInstance->getPosition()->output();
            ARMARX_DEBUG_S << "Orientation after fusion: " << fusedInstance->getOrientation()->output();
        }
        else
        {
            ARMARX_VERBOSE_S << "No old pose estimation available, using new measurement.";
            fusedInstance = newInstance->clone();
        }
 
        return fusedInstance;
    }
 
 
}