DefaultRatingModule.hpp

Go to the documentation of this file.

#pragma once

#include <ros/ros.h>
#include <boost/foreach.hpp>
#include "next_best_view/camera_model_filter/CameraModelFilter.hpp"
#include "next_best_view/helper/DebugHelper.hpp"
#include "next_best_view/helper/MathHelper.hpp"
#include "next_best_view/helper/MapHelper.hpp"
#include "next_best_view/rating/RatingModule.hpp"
#include "next_best_view/rating/impl/DefaultScoreContainer.hpp"
#include <robot_model_services/robot_model/RobotModel.hpp>

namespace next_best_view {
        class DefaultRatingModule : public RatingModule {
        private:
        /*
         * parameters to calculate the utility / costs
         */
        // the threshold for the angle between the orientation of an object and the camera orientation
        double mNormalAngleThreshold;
        // horizontal and vertical field of view in degree
        double mFovV,mFovH;
        // distance of far and near clipping plane in meters
        double mFcp,mNcp;
        // weight of the utility
        double mOmegaUtility;
        // weight of the movement costs of the pan-axis for the PTU
        double mOmegaPan;
        // weight of the movement costs of the tilt-axis for the PTU
        double mOmegaTilt;
        // weight of the rotation costs of the robot base
        double mOmegaRot;
        // weight of the translational movement costs of the robot base
        double mOmegaBase;
        // weight of the recognition costs
        double mOmegaRecognizer;

        // whether it should be checked if the calculated target robot state is reachable
        bool mUseTargetRobotState = false;

        /*
         *  parameters to disable / enable parts of utility calculation
         */
        bool mUseOrientationUtility = true;
        bool mUseProximityUtility = true;
        bool mUseSideUtility = true;

        /*
         * help members
         */
         robot_model_services::RobotModelPtr mRobotModelPtr;
        MapHelperPtr mMapHelperPtr;
        CameraModelFilterPtr mCameraModelFilterPtr;
        DebugHelperPtr mDebugHelperPtr;

        /*
         * members to cache the calculated data for one call of setBaseScoreContainer
         * must be reset everytime setBaseScoreContainer is called
         */
        // the utility for each object type
        std::map<std::string, float> mUnweightedUnnormalizedObjectUtilities;
        // the full costs of the movement - does not contain the costs of the recognition
        double mWeightedInverseMovementCosts = -1;
        // the costs of the translational movement of the base
        Precision mUnweightedInverseMovementCostsBaseTranslation = -1;
        // the costs of the rotational movement of the base
        Precision mUnweightedInverseMovementCostsBaseRotation = -1;
        // the costs of the PTU movement
        Precision mUnweightedInverseMovementCostsPTU = -1;
        // the costs of the recognition of the objects
        double mUnweightedInverseRecognitionCosts = -1;
        // the normalization value for the rating
        double mRatingNormalization = -1;
        // the PTU omega parameter
        Precision mOmegaPTU = -1;
        // the target state
        robot_model_services::RobotStatePtr mTargetState = NULL;

        /*
         * members to cache the calculated data
         */
        // the maximum recognition costs - stays the same as long as the input cloud does not change
        double mMaxRecognitionCosts;
        // whether the input cloud changed since the last time the maximum recognition costs were updated
        bool mInputCloudChanged = true;

        public:
                DefaultRatingModule();

        DefaultRatingModule(double fovV, double fovH, double fcp, double ncp, bool useTargetRobotState,
                                const robot_model_services::RobotModelPtr &robotModelPtr = robot_model_services::RobotModelPtr(),
                                const MapHelperPtr &mapHelperPtr = MapHelperPtr(),
                                const CameraModelFilterPtr &cameraModelFilterPtr = CameraModelFilterPtr());

        virtual ~DefaultRatingModule();

        void setInputCloud(const ObjectPointCloudPtr &pointCloudPtr);

        bool setBestScoreContainer(const ViewportPoint &currentViewport, ViewportPoint &candidateViewport);

        bool getBestViewport(ViewportPointCloudPtr &viewports, ViewportPoint &bestViewport);

        bool compareViewports(const ViewportPoint &a, const ViewportPoint &b);

        bool compareScoreContainer(const BaseScoreContainerPtr &a, const BaseScoreContainerPtr &b);

        BaseScoreContainerPtr getScoreContainerInstance();

        float getOrientationUtility(const ViewportPoint &viewport, ObjectPoint &objectPoint);

        float getFrustumPositionUtility(const ViewportPoint &viewport, ObjectPoint &objectPoint);

        float getNormalUtility(const ViewportPoint &viewport, const SimpleVector3 &objectNormalVector, const SimpleVector3 &objectPosition);

        float getNormalUtility(const ViewportPoint &viewport, const SimpleVector3 &objectNormalVector, const SimpleVector3 &objectPosition, double angleThreshold);

        float getProximityUtility(const ViewportPoint &viewport, const ObjectPoint &objectPoint);

        float getSideUtility(const ViewportPoint &viewport, const ObjectPoint &objectPoint);

        float getRating(const BaseScoreContainerPtr &a);

        void setNormalAngleThreshold(double angle);

        double getNormalAngleThreshold();

        float getNormalizedAngleUtility(const SimpleVector3 v1, const SimpleVector3 v2, double angleThreshold);

        void setOmegaParameters(double omegaUtility, double omegaPan, double omegaTilt, double omegaRot, double omegaBase, double omegaRecognition);

        void setFrustumParameters(double fovV, double fovH, double fcp, double ncp);

        void setUseTargetRobotState(bool useTargetRobotState);

        void setUtilityParameters(bool useOrientationUtility = true, bool useProximityUtility = true, bool useSideUtility = true);

        bool setSingleScoreContainer(const ViewportPoint &currentViewport,
                                        ViewportPoint &candidateViewport);

        void setRobotState(robot_model_services::RobotStatePtr robotState);

        void resetCache();

    private:
        float getNormalizedRating(float deviation, float threshold);

        double getUnweightedUnnormalizedUtility(const ViewportPoint &candidateViewport);

        void setUnweightedUnnormalizedObjectUtilities(const ViewportPoint &candidateViewport, std::string objectType);

        double getWeightedInverseCosts(const ViewportPoint &sourceViewport, const ViewportPoint &targetViewport);

        double getUnweightedInverseRecognitionCosts(const ViewportPoint &targetViewport);

        void setRobotState(const ViewportPoint &sourceViewport, const ViewportPoint &targetViewport);

        void setWeightedInverseMovementCosts();

        void setRatingNormalization();

        void setMaxRecognitionCosts();

        };

    typedef boost::shared_ptr<DefaultRatingModule> DefaultRatingModulePtr;
}