BaseRater.hpp

Go to the documentation of this file.

#pragma once

#include "typedef.hpp"
#include "common_type/Object.hpp"
#include "recognizer/VotedPose.hpp"
#include "RatingData.hpp"
#include "utility/GeometryHelper.hpp"

namespace ISM {
class BaseRater
{

  typedef GeometryHelper GH;

public:
        //This is what we "actually" do here: const double maxDistanceInsideBin = sqrt(3.0) * bin_size / 2;
    BaseRater(double bin_size, double maxAngleDeviation) : maxSquaredDistance(0.75 * bin_size * bin_size),
                                    maxAngleDeviation(maxAngleDeviation),
                                    projectedPose(PosePtr(new Pose())),
                                    projectedPoint(PointPtr(new Point()))
    { }

   //Methods are inlined for performance reasons.

   bool isVoteSupportingReference(VotedPosePtr& votedPose, PosePtr& refPose, RatingDataPtr data)
    {
        //Predict position of source object for current vote, using originForFitting.
         GH::getSourcePoint(refPose,
                votedPose->vote->refToObjectQuat,
                votedPose->vote->radius,
                projectedPoint);

        //Calculate distance between positions of real source object and predicted source object.
        double backProjectionSquaredDistance = GH::getSquaredDistanceBetweenPoints(votedPose->source->
                                                                                   pose->point,
                                                                                   projectedPoint);
        //Now also predict the orientation, not only the position.
        GH::getSourcePose(refPose, projectedPoint, votedPose->vote->refToObjectPoseQuat, projectedPose);

        //Calculate angle between orientation of real source object and predicted source object.
        double angleDeviation = GH::getAngleBetweenQuats(votedPose->source->pose->quat, projectedPose->quat);

        //Numerical stuff.
        if (GH::checkIfDoubleNaN(backProjectionSquaredDistance))
            backProjectionSquaredDistance = 0.0;
        if (GH::checkIfDoubleNaN(angleDeviation))
            angleDeviation = 0.0;

        //cache data for further processing
        data->angleDeviation = angleDeviation;
        data->backProjectionSquaredDistance = backProjectionSquaredDistance;

        //Reject all votes whose deviation form originForFitting exceed given thresholds. They cannot be regarded as supporting originForFitting.
        return (backProjectionSquaredDistance <= maxSquaredDistance && angleDeviation < maxAngleDeviation);

    }
  
  //Separated pure calculation of ratings from printing for performance reasons.
  virtual double rateAtBackProjectionLevel(VotedPosePtr& votedPose, RatingDataPtr data) = 0;
  virtual void printRatingAtBackProjectionLevel(VotedPosePtr& votedPose, PosePtr& refPose, RatingDataPtr data) = 0;

protected:
    const double maxSquaredDistance;
    const double maxAngleDeviation;
    PosePtr projectedPose;
    PointPtr projectedPoint;

};

typedef boost::shared_ptr<BaseRater> RaterPtr;
}