SuperqComputation Class Reference

This class provides a thread for computing in real time the superquadric, filtering the point cloud and the superquadric. More...

#include <superqComputation.h>

Inherits Thread.

Public Member Functions
	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)
	Set options for filtering the point cloud. More...
void	setSuperqFilterPar (const yarp::os::Property &newOptions, bool first_time)
	Set options for filtering the superquadric. More...
void	setIpoptPar (const yarp::os::Property &newOptions, bool first_time)
	Set options for the optimization problem solved by Ipopt. More...
yarp::os::Property	getPointsFilterPar ()
	Get options used for filtering the point cloud. More...
yarp::os::Property	getSuperqFilterPar ()
	Get options used for filtering the superquadric. More...
yarp::os::Property	getIpoptPar ()
	Get options used for solving the optimization problem with Ipopt. More...
void	setPar (const std::string &par_name, const std::string &value)
	Set a parameter equal to a value. More...
virtual bool	threadInit ()
	Init function of Thread class.
virtual void	run ()
	Run function of Thread class.
virtual void	threadRelease ()
	Release function of Thread class.
void	savePoints (const std::string &namefile, const yarp::sig::Vector &colors)
	Save point cloud used for reconstructing the superquadric. More...
bool	readPointCloud ()
	Read the object point cloud from txt for offline tests.
void	filter ()
	Filter the received point cloud.
bool	computeSuperq ()
	Compute the superquadric modeling the object. More...
void	filterSuperq ()
	Filter the superquadri with the median filter.
void	resetMedianFilter ()
	Reset median filter.
int	adaptWindComputation ()
	Compute window size according to the object velocity.
bool	configFilterSuperq ()
	Configure superquadric filter.
bool	config3Dpoints ()
	Configure point cloud filter.
yarp::sig::Vector	getSolution (bool filtered_or_not)
	Return the computed superquadric. More...
void	sendPoints (const std::deque< yarp::sig::Vector > &p)
	Get the point cloud for computing the superquadric. More...
void	getPoints3D ()
	Get the point cloud for computing the superquadric.
double	getTime ()
	Return the computation time for estimating the superquadric. More...

Data Fields
int	std_median_order
	median order width
int	max_median_order
	max median order width
yarp::sig::Vector &	x
	Object superquadric.
yarp::sig::Vector &	x_filtered
	Filtered object superquadric.
std::deque< yarp::sig::Vector > &	points
	Object point cloud.
std::deque< cv::Point >	blob_points
	Object 2D blob.

Protected Attributes
int	count
	Count variable.
bool	save_points
	Boolean variable for enabling point cloud saving.
std::string	tag_file
	Tag name of files for saving 3D points.
std::string	homeContextPath
	Path where code context is located.
yarp::os::ConstString	pointCloudFileName
	Pointcloud name file in case the module runs offline.
std::vector< cv::Point >	contour
	OpenCV variable for blob extraction.
yarp::os::BufferedPort< yarp::os::Bottle >	pointPort
	Port for receiving point cloud.
double	radius
	Radius for spatial density filter.
int	nnThreshold
	Density threshold for spatial density filter.
int	numVertices
int	median_order
	Median filder order.
int	min_median_order
	Minimum median filder order allowed.
int	new_median_order
	New median filder order estimated.
bool	filter_points
	Boolean variable for enabling point cloud filtering.
bool	fixed_window
	Boolean variable for enabling the use of a fixed window during the median filter.
bool	filter_superq
	Boolean variable for enabling superquadric filtering.
double	threshold_median
	Threshold for velocity estimation for median order.
double	min_norm_vel
	Minimum norm of velocity for considering the object moving.
bool	go_on
	Boolean variable for going to the next step of the state machine.
bool	one_shot
double	tol
	Tolerance of the optimization problem.
double	sum
double	max_cpu_time
	Max cpu time allowed for solving the optimization problem.
int	acceptable_iter
	Acceptable iter of Ipopt algorithm.
int	max_iter
	Maximum iteration allowed of Ipopt algorithm.
int	optimizer_points
	Number of 3D points used for optimization.
bool	bounds_automatic
	Boolean varibal for enabling automatic computation of variables bounds.
std::string	mu_strategy
	Mu strategy of the Ipopt algorithm.
std::string	nlp_scaling_method
	NLP scaling method of the Ipopt algorithm.
yarp::sig::Vector	elem_x
double	t_superq
	Time required for computing superquadric.
int	count_file
std::string	ob_class
	Objec class: cylinder, sphere of box.
yarp::os::ResourceFinder *	rf
double	t
double	t0
yarp::os::Mutex	mutex
yarp::os::Mutex &	mutex_shared
iCub::ctrl::MedianFilter *	mFilter
	Median filter for discarding wrong superquadrics.
iCub::ctrl::AWPolyEstimator *	PolyEst
	Time Polyestimator for estimating object velocity.
yarp::os::Property	filter_points_par
	Parameters of point cloud filter.
yarp::os::Property	filter_superq_par
	Parameters of superquadric filter.
yarp::os::Property	ipopt_par
	Parameters of the Ipopt optimization problem.

Detailed Description

This class provides a thread for computing in real time the superquadric, filtering the point cloud and the superquadric.

The optimization problem for estimating the superquadric is solved with Ipopt software package.

Definition at line 48 of file superqComputation.h.

Member Function Documentation

computeSuperq()

bool SuperqComputation::computeSuperq

(

)

Compute the superquadric modeling the object.

Returns

true/false on success/failure

Definition at line 742 of file superqComputation.cpp.

References acceptable_iter, bounds_automatic, SuperQuadric_NLP::init(), max_cpu_time, max_iter, mu_strategy, nlp_scaling_method, ob_class, optimizer_points, points, savePoints(), tag_file, tol, and x.

Referenced by run().

{
    Vector colors(3,0.0);
    colors[1]=255;

    savePoints("/3Dpoints-"+tag_file, colors);

    Ipopt::SmartPtr<Ipopt::IpoptApplication> app=new Ipopt::IpoptApplication;
    app->Options()->SetNumericValue("tol",tol);
    app->Options()->SetIntegerValue("acceptable_iter",acceptable_iter);
    app->Options()->SetStringValue("mu_strategy",mu_strategy);
    app->Options()->SetIntegerValue("max_iter",max_iter);
    app->Options()->SetNumericValue("max_cpu_time",max_cpu_time);
    app->Options()->SetStringValue("nlp_scaling_method",nlp_scaling_method);
    app->Options()->SetStringValue("hessian_approximation","limited-memory");
    app->Options()->SetIntegerValue("print_level",0);
    app->Initialize();

    Ipopt::SmartPtr<SuperQuadric_NLP> superQ_nlp= new SuperQuadric_NLP;

    superQ_nlp->init();
    superQ_nlp->configure(this->rf,bounds_automatic, ob_class);

    superQ_nlp->setPoints(points, optimizer_points);

    double t0_superq=Time::now();

    yDebug()<<"[SuperqComputation]: Start IPOPT ";

    Ipopt::ApplicationReturnStatus status=app->OptimizeTNLP(GetRawPtr(superQ_nlp));

    yDebug()<<"[SuperqComputation]: Finish IPOPT ";

    double t_s=Time::now()-t0_superq;

    if (status==Ipopt::Solve_Succeeded)
    {
        x=superQ_nlp->get_result();
        yInfo("[SuperqComputation]: Solution of the optimization problem: %s", x.toString(3,3).c_str());
        yInfo("[SuperqComputation]: Execution time : %f", t_s);
        return true;
    }
    else if(status==Ipopt::Maximum_CpuTime_Exceeded)
    {
        x=superQ_nlp->get_result();
        yWarning("[SuperqComputation]: Solution after maximum time exceeded: %s", x.toString(3,3).c_str());
        return true;
    }
    else
    {
        x.resize(11,0.0);
        return false;
    }
}

getIpoptPar()

Property SuperqComputation::getIpoptPar

(

)

Get options used for solving the optimization problem with Ipopt.

Returns

a property with the options on ipopt / /*

Definition at line 418 of file superqComputation.cpp.

References acceptable_iter, max_cpu_time, max_iter, mu_strategy, nlp_scaling_method, optimizer_points, and tol.

{
    LockGuard lg(mutex);

    Property advOptions;
    advOptions.put("optimizer_points",optimizer_points);
    advOptions.put("max_cpu_time",max_cpu_time);
    advOptions.put("tol",tol);
    advOptions.put("max_iter",max_iter);
    advOptions.put("acceptable_iter",acceptable_iter);
    advOptions.put("IPOPT_mu_strategy",mu_strategy);
    advOptions.put("IPOPT_nlp_scaling_method",nlp_scaling_method);
    return advOptions;
}

getPointsFilterPar()

Property SuperqComputation::getPointsFilterPar

(

)

Get options used for filtering the point cloud.

Returns

a property with the options on the points filtering

Definition at line 120 of file superqComputation.cpp.

References nnThreshold, and radius.

{
    LockGuard lg(mutex);

    Property advOptions;
    advOptions.put("filter_radius",radius);
    advOptions.put("filter_nnThreshold",nnThreshold);
    return advOptions;
}

getSolution()

Vector SuperqComputation::getSolution

(

bool

filtered_or_not

)

Return the computed superquadric.

Parameters

filtered_or_not

can be "on" or "off"

Returns

the estimated superquadric filtered or not

Definition at line 867 of file superqComputation.cpp.

References x, and x_filtered.

{
    LockGuard lg(mutex);

    if (filtered_superq==false)
        return x;
    else
        return x_filtered;
}

getSuperqFilterPar()

Property SuperqComputation::getSuperqFilterPar

(

)

Get options used for filtering the superquadric.

Returns

a property with the options on superquadric filtering / /*

Definition at line 259 of file superqComputation.cpp.

References fixed_window, max_median_order, min_median_order, min_norm_vel, std_median_order, and threshold_median.

{
    LockGuard lg(mutex);

    Property advOptions;
    if (fixed_window)
        advOptions.put("fixed_window","on");
    else
        advOptions.put("fixed_window","off");
    advOptions.put("median_order",std_median_order);
    advOptions.put("min_median_order",min_median_order);
    advOptions.put("max_median_order",max_median_order);
    advOptions.put("threshold_median",threshold_median);
    advOptions.put("min_norm_vel",min_norm_vel);
    return advOptions;
}

getTime()

double SuperqComputation::getTime

(

)

Return the computation time for estimating the superquadric.

Returns

the period value

Definition at line 451 of file superqComputation.cpp.

References t_superq.

{   
    LockGuard lg(mutex);
    return t_superq;
}

savePoints()

void SuperqComputation::savePoints	(	const std::string &	namefile,
		const yarp::sig::Vector &	colors
	)

Save point cloud used for reconstructing the superquadric.

Parameters

namefile	is the name of the file where to save the superquadric
colors	is a Vector with the color to be used for saving the point cloud

Definition at line 622 of file superqComputation.cpp.

References homeContextPath, points, and save_points.

Referenced by computeSuperq(), and filter().

{
    if (save_points)
    {
        ofstream fout;
        stringstream ss;
        ss<<count_file;
        string count_file_str=ss.str();
        fout.open((homeContextPath+namefile+count_file_str+".off").c_str());

        if (fout.is_open())
        {
            fout<<"COFF"<<endl;
            fout<<points.size()<<" 0 0"<<endl;
            fout<<endl;
            for (size_t i=0; i<points.size(); i++)
            {
                int r=points[i][3];
                int g=points[i][4];
                int b=points[i][5];
                fout<<points[i].subVector(0,2).toString(3,3)<<" "<<r<<" "<<g<<" "<<b<<endl;
            }

            fout<<endl;
        }
        else
            yError()<<"[SuperqComputation]: Some problems in opening output file!";

        fout.close();

        count_file++;
    }
}

sendPoints()

void SuperqComputation::sendPoints

(

const std::deque< yarp::sig::Vector > &

p

)

Get the point cloud for computing the superquadric.

Parameters

p	is the object point cloud

Definition at line 878 of file superqComputation.cpp.

References points.

{
    LockGuard lg(mutex);

    points.clear();

    for (int i=0; i<p.size(); i++)
    {
        points.push_back(p[i]);
    }
}

setIpoptPar()

void SuperqComputation::setIpoptPar	(	const yarp::os::Property &	newOptions,
		bool	first_time
	)

Set options for the optimization problem solved by Ipopt.

Parameters

newOptions	is a Property with the new options to set
first_time	takes into account if the options have already been set or not

Definition at line 277 of file superqComputation.cpp.

References acceptable_iter, max_cpu_time, max_iter, mu_strategy, nlp_scaling_method, optimizer_points, points, and tol.

Referenced by threadInit().

{
    LockGuard lg(mutex);

    int points=newOptions.find("optimizer_points").asInt();
    if (newOptions.find("optimizer_points").isNull() && (first_time==true))
    {
        optimizer_points=50;
    }
    else if (!newOptions.find("optimizer_points").isNull())
    {
        if ((points>=10) && (points<=300))
        {
            optimizer_points=points;
        }
        else if (points<10)
        {
            optimizer_points=10;
        }
        else if (points>300)
        {
            optimizer_points=300;
        }
    }

    double maxCpuTime=newOptions.find("max_cpu_time").asDouble();
    if (newOptions.find("max_cpu_time").isNull() && (first_time==true))
    {
        max_cpu_time=5.0;
    }
    else if (!newOptions.find("max_cpu_time").isNull())
    {
        if ((maxCpuTime>=0.01) && (maxCpuTime<=10.0))
        {
            max_cpu_time=maxCpuTime;
        }
        else if (maxCpuTime<0.01)
        {
            max_cpu_time=0.01;
        }
        else if (maxCpuTime>10.0)
        {
            max_cpu_time=10.0;
        }
    }

    double tolValue=newOptions.find("tol").asDouble();
    if (newOptions.find("tol").isNull() && (first_time==true))
    {
        tol=1e-5;
    }
    else if (!newOptions.find("tol").isNull())
    {
        if ((tolValue>1e-8) && (tolValue<=0.01))
        {
            tol=tolValue;
        }
        else if (tolValue<1e-8)
        {
            tol=1e-8;
        }
        else if (tolValue>0.01)
        {
            tol=0.01;
        }
    }

    int accIter=newOptions.find("acceptable_iter").asInt();
    if (newOptions.find("acceptable_iter").isNull() && (first_time==true))
    {
        acceptable_iter=0;
    }
    else if (!newOptions.find("acceptable_iter").isNull())
    {
        if ((accIter>=0 )&& (accIter<=10))
        {
             acceptable_iter=accIter;
        }
        else if (accIter<0)
        {
            acceptable_iter=0;
        }
        else if (accIter>10)
        {
            acceptable_iter=10;
        }
    }

    int maxIter=newOptions.find("max_iter").asInt();
    if (newOptions.find("max_iter").isNull() && (first_time==true))
    {
        max_iter=100;
    }
    else if (!newOptions.find("max_iter").isNull())
    {
        if ((maxIter>1))
        {
            max_iter=maxIter;
        }
        else
        {
            max_iter=100;
        }
    }

    string mu_str=newOptions.find("mu_strategy").asString();
    if (newOptions.find("mu_strategy").isNull() && (first_time==true))
    {
        mu_strategy="monotone";
    }
    else if (!newOptions.find("mu_strategy").isNull())
    {
        if ((mu_str=="adaptive") || (mu_str=="monotone"))
        {
            mu_strategy=mu_str;
        }
        else
        {
            mu_strategy="monotone";
        }
    }

    string nlp=newOptions.find("nlp_scaling_method").asString();
    if (newOptions.find("nlp_scaling_method").isNull() && (first_time==true))
    {
        nlp_scaling_method="gradient-based";
    }
    else if (!newOptions.find("nlp_scaling_method").isNull())
    {
        if ((nlp=="none") || (nlp=="gradient-based"))
        {
            nlp_scaling_method=nlp;
        }
        else
        {
            nlp_scaling_method="gradient-based";
        }
    }
}

setPar()

void SuperqComputation::setPar	(	const std::string &	par_name,
		const std::string &	value
	)

Set a parameter equal to a value.

Parameters

par_name	is the name of the variable we want to change
value	is the new balue

Definition at line 434 of file superqComputation.cpp.

References filter_points, filter_superq, ob_class, save_points, and tag_file.

{
    if (par_name=="tag_file")
        tag_file=value;
    else if (par_name=="filter_points")
        filter_points=(value=="on");
    else if (par_name=="filter_superq")
        filter_superq=(value=="on");
    else if (par_name=="save_points")
        save_points=(value=="on");
    else if (par_name=="one_shot")
        one_shot=(value=="on");
    else if (par_name=="object_class")
        ob_class=value;
}

setPointsFilterPar()

void SuperqComputation::setPointsFilterPar	(	const yarp::os::Property &	newOptions,
		bool	first_time
	)

Set options for filtering the point cloud.

Parameters

newOptions	is a Property with the new options to set
first_time	takes into account if the options have already been set or not

Definition at line 76 of file superqComputation.cpp.

References nnThreshold, and radius.

Referenced by threadInit().

{
    LockGuard lg(mutex);

    double radiusValue=newOptions.find("filter_radius").asDouble();
    if (newOptions.find("filter_radius").isNull() && (first_time==true))
    {
        radius=0.01;
    }
    else if (!newOptions.find("filter_radius").isNull())
    {
        if ((radiusValue>0.0000001) && (radiusValue<0.01))
        {
            radius=radiusValue;
        }
        else if ((radiusValue>=0.01))
        {
            radius=0.01;
        }
        else if ((radiusValue<=0.0000001))
        {
            radius=0.0000001;
        }
    }

   int nnThreValue=newOptions.find("filter_nnThreshold").asInt();
    if (newOptions.find("filter_nnThreshold").isNull() && (first_time==true))
    {
        nnThreshold=100;
    }
    else if (!newOptions.find("filter_nnThreshold").isNull())
    {
        if ((nnThreValue>0) && (nnThreValue<100))
        {
                nnThreshold=nnThreValue;
        }
        else
        {
            nnThreshold=100;
        }
    }
}

setSuperqFilterPar()

void SuperqComputation::setSuperqFilterPar	(	const yarp::os::Property &	newOptions,
		bool	first_time
	)

Set options for filtering the superquadric.

Parameters

newOptions	is a Property with the new options to set
first_time	takes into account if the options have already been set or not

Definition at line 131 of file superqComputation.cpp.

References fixed_window, max_median_order, min_median_order, min_norm_vel, std_median_order, and threshold_median.

Referenced by threadInit().

{
    LockGuard lg(mutex);

    int mOrderValue=newOptions.find("median_order").asInt();
    if (newOptions.find("median_order").isNull() && (first_time==true))
    {
        std_median_order=3;
    }
    else if (!newOptions.find("median_order").isNull())
    {
        if((mOrderValue>=1) && (mOrderValue<=50))
        {
            std_median_order=mOrderValue;
        }
        else if (mOrderValue<1)
        {
            std_median_order=1;
        }
        else if (mOrderValue>50)
        {
            std_median_order=50;
        }
    }

    mOrderValue=newOptions.find("min_median_order").asInt();
    if (newOptions.find("min_median_order").isNull() && (first_time==true))
    {
        min_median_order=1;
    }
    else if (!newOptions.find("min_median_order").isNull())
    {
        if ((mOrderValue>=1) && (mOrderValue<=50))
        {
            min_median_order=mOrderValue;
        }
        else if (mOrderValue<1)
        {
            min_median_order=1;
        }
        else if (mOrderValue>50)
        {
            min_median_order=50;
        }
    }

    mOrderValue=newOptions.find("max_median_order").asInt();
    if (newOptions.find("max_median_order").isNull() && (first_time==true))
    {
        max_median_order=30;
    }
    else if (!newOptions.find("max_median_order").isNull())
    {
        if ((mOrderValue>=1) && (mOrderValue<=50) && (mOrderValue>=min_median_order))
        {
            max_median_order=mOrderValue;
        }
        else if ((mOrderValue<1) || (mOrderValue<min_median_order))
        {
            max_median_order=min_median_order;
        }
        else if (mOrderValue>50)
        {
            max_median_order=50;
        }
    }

    double threValue=newOptions.find("threshold_median").asDouble();
    if (newOptions.find("threhsold_median").isNull() && (first_time==true))
    {
        threshold_median=0.1;
    }
    else if (!newOptions.find("threhsold_median").isNull())
    {
        if ((threValue>0.005) && (threValue<=2.0))
        {
            threshold_median=threValue;
        }
        else if (threValue<0.005)
        {
            threshold_median=0.005;
        }
        else if (threValue>2.0)
        {
            threshold_median=2.0;
        }
    }

    double minNormVel=newOptions.find("min_norm_vel").asDouble();
    if (newOptions.find("min_norm_vel").isNull() && (first_time==true))
    {
        min_norm_vel=0.01;
    }
    else if (!newOptions.find("min_norm_vel").isNull())
    {
        if ((minNormVel>0.005) && (minNormVel<=0.1))
        {
            min_norm_vel=minNormVel;
        }
        else if (minNormVel<0.005)
        {
            min_norm_vel=0.005;
        }
        else if (minNormVel>0.1)
        {
            min_norm_vel=0.1;
        }
    }

    string par=newOptions.find("fixed_window").asString();
    if (newOptions.find("fixed_window").isNull() && (first_time==true))
    {
        fixed_window=false;
    }
    else if (!newOptions.find("fixed_window").isNull())
    {
        if ((par=="on") || (par=="off"))
        {
            fixed_window=(par=="on");
        }
        else
        {
            fixed_window=false;
        }
    }
}

---

The documentation for this class was generated from the following files:

• C:/Users/upattacini/Desktop/superquadric-model/include/superqComputation.h
• C:/Users/upattacini/Desktop/superquadric-model/src/superqComputation.cpp