KalmanFilterFusion.cpp

Go to the documentation of this file.

#include "KalmanFilterFusion.h"
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
 
#include <RobotAPI/libraries/core/LinkedPose.h>
 
#include <MemoryX/libraries/helpers/EarlyVisionHelpers/EarlyVisionConverters.h>
#include <MemoryX/libraries/helpers/EarlyVisionHelpers/NoMotionFilter.h>
#include <MemoryX/libraries/motionmodels/AbstractMotionModel.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;
    }
 
 
} // namespace memoryx