superqComputation.h

/*
 * Copyright (C) 2015 iCub Facility - Istituto Italiano di Tecnologia
 * Author: Giulia Vezzani
 * email:  giulia.vezzani@iit.it
 * Permission is granted to copy, distribute, and/or modify this program
 * under the terms of the GNU General Public License, version 2 or any
 * later version published by the Free Software Foundation.
 *
 * A copy of the license can be found at
 * http://www.robotcub.org/icub/license/gpl.txt
 *
 * This program 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
*/

#ifndef __COMPUTATION_H__
#define __COMPUTATION_H__

#include <yarp/dev/all.h>

#include <opencv2/opencv.hpp>

#include <iCub/ctrl/filters.h>
#include <iCub/ctrl/adaptWinPolyEstimator.h>

#include "superquadric.h"

/*******************************************************************************/
class SpatialDensityFilter
{
public:

    /*******************************************************************************/
    static std::vector<int>  filter(const cv::Mat &data,const double radius, const int maxResults, std::deque<yarp::sig::Vector> &points);
};

/*******************************************************************************/
class SuperqComputation : public yarp::os::Thread
{
protected:

    int count;
    bool save_points;
    std::string tag_file;
    std::string homeContextPath;
    yarp::os::ConstString pointCloudFileName;
    std::vector<cv::Point> contour;

    yarp::os::BufferedPort<yarp::os::Bottle> pointPort;

    // Filters parameters
    double radius;
    int nnThreshold;
    int numVertices;
    int median_order;  
    int min_median_order;   
    int new_median_order;
    bool filter_points;
    bool fixed_window;  
    bool filter_superq;
    double threshold_median;
    double min_norm_vel;

    // On/off parameters
    bool go_on;
    bool one_shot;

    // Options for optimization with ipopt
    double tol;
    double sum;
    double max_cpu_time;
    int acceptable_iter;
    int max_iter;
    int optimizer_points;
    bool bounds_automatic;
    std::string mu_strategy;
    std::string nlp_scaling_method;
    yarp::sig::Vector elem_x;

    double t_superq;
    int count_file;
    std::string ob_class;
    
    yarp::os::ResourceFinder *rf;
    double t,t0;
    yarp::os::Mutex mutex;
    yarp::os::Mutex &mutex_shared;

    // Classes uses for filtering the superquadric
    iCub::ctrl::MedianFilter *mFilter;
    iCub::ctrl::AWPolyEstimator *PolyEst;

    // Properties with all the options
    yarp::os::Property filter_points_par;
    yarp::os::Property filter_superq_par;
    yarp::os::Property ipopt_par;
public:

    int std_median_order;
    int max_median_order;

    yarp::sig::Vector &x;
    yarp::sig::Vector &x_filtered;
    std::deque<yarp::sig::Vector> &points;
    std::deque<cv::Point> blob_points;

    /***********************************************************************/
    SuperqComputation(yarp::os::Mutex &mutex_shared, int _rate, bool _filter_points, bool _filter_superq, bool _fixed_window, std::deque<yarp::sig::Vector> &_points,
                      std::string _tag_file, double _threshold_median, const yarp::os::Property &filters_points_par, yarp::sig::Vector &_x, yarp::sig::Vector &_x_filtered,
                      const yarp::os::Property &filters_superq_par, const yarp::os::Property &optimizer_par, const std::string &_homeContextPath, bool _save_points, yarp::os::ResourceFinder *rf);

    /***********************************************************************/
    void setPointsFilterPar(const yarp::os::Property &newOptions, bool first_time);

    /***********************************************************************/
    void setSuperqFilterPar(const yarp::os::Property &newOptions, bool first_time);

    /***********************************************************************/
    void setIpoptPar(const yarp::os::Property &newOptions, bool first_time);

    /***********************************************************************/
    yarp::os::Property getPointsFilterPar();

    yarp::os::Property getSuperqFilterPar();

    yarp::os::Property getIpoptPar();

    /***********************************************************************/
    void setPar(const std::string &par_name, const std::string &value);

    /***********************************************************************/
    virtual bool threadInit();

    /***********************************************************************/
    virtual void run();

    /***********************************************************************/
    virtual void threadRelease();

    /***********************************************************************/
    void savePoints(const std::string &namefile, const yarp::sig::Vector &colors);

    /***********************************************************************/
    bool readPointCloud();

    /***********************************************************************/
    void filter();

    /***********************************************************************/
    bool computeSuperq();

    /***********************************************************************/
    void filterSuperq();

    /***********************************************************************/
    void resetMedianFilter();

    /***********************************************************************/
    int adaptWindComputation();

    /***********************************************************************/
    bool configFilterSuperq();

    /***********************************************************************/
    bool config3Dpoints();

    /***********************************************************************/
    yarp::sig::Vector getSolution(bool filtered_or_not);

    /***********************************************************************/
    void sendPoints(const std::deque<yarp::sig::Vector> &p);

    /***********************************************************************/
    void getPoints3D();

    /***********************************************************************/
    double getTime();     
};

#endif