SuperQuadric_NLP Class Reference

This class solves the optimization problem with the Ipopt software package and returns the estiamted superquadric, better fitting a given point cloud. More...

#include <superquadric.h>

Inherits TNLP.

Public Member Functions
void	init ()
	Init function.
void	setPoints (const std::deque< yarp::sig::Vector > &point_cloud, const int &optimizer_points)
	Set point to be used for superquadric estimation. More...
void	configure (yarp::os::ResourceFinder *rf, bool bounds_aut, const std::string &object_class)
	Configure function. More...
yarp::sig::Vector	get_result () const
	Extract the solution. More...
bool	readMatrix (const std::string &tag, yarp::sig::Matrix &matrix, const int &dimension, yarp::os::ResourceFinder *rf)
	Function for reading matrices from config files. More...

Data Fields
yarp::sig::Vector	solution
	Final solution.
std::deque< yarp::sig::Vector >	points_downsampled
	3D points actually used for superquadric estimation

Protected Member Functions
bool	get_nlp_info (Ipopt::Index &n, Ipopt::Index &m, Ipopt::Index &nnz_jac_g, Ipopt::Index &nnz_h_lag, Ipopt::TNLP::IndexStyleEnum &index_style)
	Get info for the nonlinear problem to be solved with ipopt. More...
void	computeBounds ()
	Compute bounds variable from the point cloud for speeding up optimization.
bool	get_bounds_info (Ipopt::Index n, Ipopt::Number *x_l, Ipopt::Number *x_u, Ipopt::Index m, Ipopt::Number *g_l, Ipopt::Number *g_u)
	Get variable bounds for the nonlinear problem to be solved with ipopt. More...
bool	get_starting_point (Ipopt::Index n, bool init_x, Ipopt::Number *x, bool init_z, Ipopt::Number *z_L, Ipopt::Number *z_U, Ipopt::Index m, bool init_lambda, Ipopt::Number *lambda)
	Get the starting point for the nonlinear problem to be solved with ipopt. More...
bool	eval_f (Ipopt::Index n, const Ipopt::Number *x, bool new_x, Ipopt::Number &obj_value)
	Cost function of the nonlinear problem to be solved with ipopt. More...
void	F (const Ipopt::Number *x, std::deque< yarp::sig::Vector > &points, bool &new_x)
	Auxiliary function for computing cost function of the nonlinear problem to be solved with ipopt. More...
double	f (const Ipopt::Number *x, const yarp::sig::Matrix &R, const yarp::sig::Vector &point_cloud)
	Auxiliary function for computing cost function of the nonlinear problem to be solved with ipopt. More...
double	F_v (const yarp::sig::Vector &x, const std::deque< yarp::sig::Vector > &points)
	Auxiliary function for computing the gradient of cost function of the nonlinear problem. More...
double	f_v (const yarp::sig::Vector &x, const yarp::sig::Matrix &R, const yarp::sig::Vector &point_cloud)
	Auxiliary function for computing cost function of the nonlinear problem to be solved with ipopt. More...
bool	eval_grad_f (Ipopt::Index n, const Ipopt::Number *x, bool new_x, Ipopt::Number *grad_f)
	Gradient of the cost function of the nonlinear problem. More...
bool	eval_g (Ipopt::Index n, const Ipopt::Number *x, bool new_x, Ipopt::Index m, Ipopt::Number *g)
	Constraints of the nonlinear problem. More...
bool	eval_jac_g (Ipopt::Index n, const Ipopt::Number *x, bool new_x, Ipopt::Index m, Ipopt::Index nele_jac, Ipopt::Index *iRow, Ipopt::Index *jCol, Ipopt::Number *values)
	Jacobian of the constraints of the nonlinear problem. More...
void	computeX0 (yarp::sig::Vector &x0, std::deque< yarp::sig::Vector > &point_cloud)
	Compute a good starting point for the nonlinear problem. More...
void	computeInitialOrientation (yarp::sig::Vector &x0, std::deque< yarp::sig::Vector > &point_cloud)
	Compute initial superquadric orientation from the point cloud. More...
yarp::sig::Matrix	computeBoundingBox (std::deque< yarp::sig::Vector > &points, const yarp::sig::Vector &x0)
	Compute bounding box from the point cloud. More...
void	finalize_solution (Ipopt::SolverReturn status, Ipopt::Index n, const Ipopt::Number *x, const Ipopt::Number *z_L, const Ipopt::Number *z_U, Ipopt::Index m, const Ipopt::Number *g, const Ipopt::Number *lambda, Ipopt::Number obj_value, const Ipopt::IpoptData *ip_data, Ipopt::IpoptCalculatedQuantities *ip_cq)
	Finalize the solution. More...

Protected Attributes
bool	bounds_automatic
	Boolean variable for enabling automatic bounds computation.
yarp::sig::Vector	x_v
	Auxiliar vector for gradient computation.
yarp::sig::Vector	x0
	Starting point for the optimization problem.
yarp::sig::Matrix	bounds
	Bounds variable of the optimization problem.
double	aux_objvalue
std::string	obj_class
	Object class: cylinder, sphere and box.
yarp::os::ResourceFinder *	rf

Detailed Description

This class solves the optimization problem with the Ipopt software package and returns the estiamted superquadric, better fitting a given point cloud.

Definition at line 36 of file superquadric.h.

Member Function Documentation

computeBoundingBox()

Matrix SuperQuadric_NLP::computeBoundingBox ( std::deque< yarp::sig::Vector > &  points, const yarp::sig::Vector &  x0  )

protected

Compute bounding box from the point cloud.

Parameters

points	is the point cloud
x0	is the initial value for the superquadric to be estimated

Returns

a matrix with the variable boundss

Definition at line 344 of file superquadric.cpp.

{
    Matrix BB(3,2);
    Matrix R3(3,3);

    R3=euler2dcm(x0.subVector(8,10)).submatrix(0,2,0,2);

    Vector point(3,0.0);
    point=R3.transposed()*points[0];

    BB(0,0)=point[0];
    BB(1,0)=point[1];
    BB(2,0)=point[2];
    BB(0,1)=point[0];
    BB(1,1)=point[1];
    BB(2,1)=point[2];

    for (size_t i=0; i<points.size();i++)
    {
        Vector &pnt=points[i];
        point=R3.transposed()*pnt;
        if(BB(0,0)>point[0])
           BB(0,0)=point[0];

        if(BB(0,1)<point[0])
            BB(0,1)=point[0];

        if(BB(1,0)>point[1])
            BB(1,0)=point[1];

        if(BB(1,1)<point[1])
            BB(1,1)=point[1];

        if(BB(2,0)>point[2])
            BB(2,0)=point[2];

        if(BB(2,1)<point[2])
            BB(2,1)=point[2];
    }

    return BB;
}

computeInitialOrientation()

void SuperQuadric_NLP::computeInitialOrientation ( yarp::sig::Vector &  x0, std::deque< yarp::sig::Vector > &  point_cloud  )

protected

Compute initial superquadric orientation from the point cloud.

Parameters

x0	is the initial value for the superquadric to be estimated
point_cloud	is the object point cloud

Definition at line 297 of file superquadric.cpp.

{
    Matrix M=zeros(3,3);
    Matrix R(3,3);
    Matrix u(3,3);
    Matrix v(3,3);

    Vector s(3,0.0);
    Vector n(3,0.0);
    Vector o(3,0.0);
    Vector a(3,0.0);

    for (size_t i=0;i<point_cloud.size(); i++)
    {
        Vector &point=point_cloud[i];
        M(0,0)= M(0,0) + (point[1]-x0[6])*(point[1]-x0[6]) + (point[2]-x0[7])*(point[2]-x0[7]);
        M(0,1)= M(0,1) - (point[1]-x0[6])*(point[0]-x0[5]);
        M(0,2)= M(0,2) - (point[2]-x0[7])*(point[0]-x0[5]);
        M(1,1)= M(1,1) + (point[0]-x0[5])*(point[0]-x0[5]) + (point[2]-x0[7])*(point[2]-x0[7]);
        M(2,2)= M(2,2) + (point[1]-x0[6])*(point[1]-x0[6]) + (point[0]-x0[5])*(point[0]-x0[5]);
        M(1,2)= M(1,2) - (point[2]-x0[7])*(point[1]-x0[6]);
    }

    M(0,0)= M(0,0)/point_cloud.size();
    M(0,1)= M(0,1)/point_cloud.size();
    M(0,2)= M(0,2)/point_cloud.size();
    M(1,1)= M(1,1)/point_cloud.size();
    M(2,2)= M(2,2)/point_cloud.size();
    M(1,2)= M(1,2)/point_cloud.size();

    M(1,0)= M(0,1);
    M(2,0)= M(0,2);
    M(2,1)= M(1,2);

    SVDJacobi(M,u,s,v);
    n=u.getCol(0);
    o=u.getCol(1);
    a=u.getCol(2);

    R.setCol(0,n);
    R.setCol(1,o);
    R.setCol(2,a);

    x0.setSubvector(8,dcm2euler(R));
}

computeX0()

void SuperQuadric_NLP::computeX0 ( yarp::sig::Vector &  x0, std::deque< yarp::sig::Vector > &  point_cloud  )

protected

Compute a good starting point for the nonlinear problem.

Parameters

x0	is the initial value for the superquadric to be estimated
point_cloud	is the object point cloud

Definition at line 266 of file superquadric.cpp.

{
    x0[3]=1.0;
    x0[4]=1.0;
    x0[5]=0.0;
    x0[6]=0.0;
    x0[7]=0.0;

    for (size_t i=0; i<point_cloud.size();i++)
    {
        Vector &point=point_cloud[i];
        x0[5]+=point[0];
        x0[6]+=point[1];
        x0[7]+=point[2];
    }

    x0[5]/=point_cloud.size();
    x0[6]/=point_cloud.size();
    x0[7]/=point_cloud.size();

    computeInitialOrientation(x0,point_cloud);

    Matrix bounding_box(3,2);
    bounding_box=computeBoundingBox(point_cloud,x0);

    x0[0]=(-bounding_box(0,0)+bounding_box(0,1))/2;
    x0[1]=(-bounding_box(1,0)+bounding_box(1,1))/2;
    x0[2]=(-bounding_box(2,0)+bounding_box(2,1))/2;
}

configure()

void SuperQuadric_NLP::configure	(	yarp::os::ResourceFinder *	rf,
		bool	bounds_aut,
		const std::string &	object_class
	)

Configure function.

Parameters

rf	is the resource finder
bounds_aut	is to set or not the automatic computation of the variable bound
object_class	is the object class according to its shape

Definition at line 255 of file superquadric.cpp.

{
    bounds.resize(11,2);

    bounds_automatic=b_automatic;
    obj_class=object_class;

    readMatrix("bounds_"+object_class,bounds, 11, rf);
}

eval_f()

bool SuperQuadric_NLP::eval_f ( Ipopt::Index  n, const Ipopt::Number *  x, bool  new_x, Ipopt::Number &  obj_value  )

protected

Cost function of the nonlinear problem to be solved with ipopt.

Parameters

n	is the dimension of the variable
x	is the variable
new_x	takes into account is the variable has been updated or not
obj_value	is the value of the cost function true

Definition at line 138 of file superquadric.cpp.

 {
     F(x,points_downsampled, new_x);
     obj_value=aux_objvalue;
     return true;
 }

eval_g()

bool SuperQuadric_NLP::eval_g ( Ipopt::Index  n, const Ipopt::Number *  x, bool  new_x, Ipopt::Index  m, Ipopt::Number *  g  )

protected

Constraints of the nonlinear problem.

Parameters

n	is the dimension of the variable
x	is the variable
m	is the number of constraints
new_x	takes into account is the variable has been updated or not
g	is the values of the constraints

Returns

true

Definition at line 240 of file superquadric.cpp.

 {
     return false;
 }

eval_grad_f()

bool SuperQuadric_NLP::eval_grad_f ( Ipopt::Index  n, const Ipopt::Number *  x, bool  new_x, Ipopt::Number *  grad_f  )

protected

Gradient of the cost function of the nonlinear problem.

Parameters

x	is the variable
n	is the dimension of the variable
new_x	takes into account is the variable has been updated or not
grad_f	is the gradient of the cost function

Definition at line 213 of file superquadric.cpp.

 {
     Vector x_tmp(n,0.0);
     double grad_p, grad_n;
     double eps=1e-8;

     for (Ipopt::Index j=0;j<n;j++)
         x_tmp[j]=x[j];

     for (Ipopt::Index j=0;j<n;j++)
     {
         x_tmp[j]+=eps;

         grad_p=F_v(x_tmp,points_downsampled);

         x_tmp[j]-=eps;

         grad_n=F_v(x_tmp,points_downsampled);

         grad_f[j]=(grad_p-grad_n)/(2.0*eps);
      }

     return true;
 }

eval_jac_g()

bool SuperQuadric_NLP::eval_jac_g ( Ipopt::Index  n, const Ipopt::Number *  x, bool  new_x, Ipopt::Index  m, Ipopt::Index  nele_jac, Ipopt::Index *  iRow, Ipopt::Index *  jCol, Ipopt::Number *  values  )

protected

Jacobian of the constraints of the nonlinear problem.

Parameters

n	is the dimension of the variable
x	is the variable
m	is the number of constraints
new_x	takes into account is the variable has been updated or not
iRow	contains the jacobian raws
iCol	contains the jacobian columns
values	contains the jacobian values

Returns

true

Definition at line 247 of file superquadric.cpp.

 {
     return false;
 }

F()

void SuperQuadric_NLP::F ( const Ipopt::Number *  x, std::deque< yarp::sig::Vector > &  points, bool &  new_x  )

protected

Auxiliary function for computing cost function of the nonlinear problem to be solved with ipopt.

Parameters

x	is the variable
points_on	is object point cloud
new_x	takes into account is the variable has been updated or not the cost function value

Definition at line 147 of file superquadric.cpp.

 {
     if (new_x)
     {
         double value=0.0;

         Vector euler(3,0.0);
         euler[0]=x[8];
         euler[1]=x[9];
         euler[2]=x[10];
         Matrix R=euler2dcm(euler);

         for(size_t i=0;i<points.size();i++)
         {
             double tmp=pow(f(x,R,points[i]),x[3])-1;
             value+=tmp*tmp;
         }
         value*=x[0]*x[1]*x[2]/points.size();
         aux_objvalue=value;
     }
 }

f()

double SuperQuadric_NLP::f ( const Ipopt::Number *  x, const yarp::sig::Matrix &  R, const yarp::sig::Vector &  point_cloud  )

protected

Auxiliary function for computing cost function of the nonlinear problem to be solved with ipopt.

Parameters

x	is the variable
point	is one point of the point cloud

Returns

a part of the cost function value

Definition at line 170 of file superquadric.cpp.

 {
     double num1 = R(0, 0)*point_cloud[0] + R(0, 1)*point_cloud[1] + R(0, 2)*point_cloud[2] - x[5] * R(0, 0) - x[6] * R(0, 1) - x[7] * R(0, 2);
     double num2 = R(1, 0)*point_cloud[0] + R(1, 1)*point_cloud[1] + R(1, 2)*point_cloud[2] - x[5] * R(1, 0) - x[6] * R(1, 1) - x[7] * R(1, 2);
     double num3 = R(2, 0)*point_cloud[0] + R(2, 1)*point_cloud[1] + R(2, 2)*point_cloud[2] - x[5] * R(2, 0) - x[6] * R(2, 1) - x[7] * R(2, 2);
     double tmp = pow(abs(num1 / x[0]), 2.0 / x[4]) + pow(abs(num2 / x[1]), 2.0 / x[4]);
     return pow(abs(tmp), x[4] / x[3]) + pow(abs(num3 / x[2]), (2.0 / x[3]));
 }

F_v()

double SuperQuadric_NLP::F_v ( const yarp::sig::Vector &  x, const std::deque< yarp::sig::Vector > &  points  )

protected

Auxiliary function for computing the gradient of cost function of the nonlinear problem.

Parameters

x	is the variable
points_on	is one point of object point cloud

Returns

cost function value

Definition at line 181 of file superquadric.cpp.

 {
     double value=0.0;

     Vector euler(3,0.0);
     euler[0]=x[8];
     euler[1]=x[9];
     euler[2]=x[10];
     Matrix R=euler2dcm(euler);

     for (size_t i=0;i<points.size();i++)
     {
          double tmp=pow(f_v(x,R,points[i]),x[3])-1;
          value+=tmp*tmp;
     }

     value*=x[0]*x[1]*x[2]/points.size();
     return value;
 }

f_v()

double SuperQuadric_NLP::f_v ( const yarp::sig::Vector &  x, const yarp::sig::Matrix &  R, const yarp::sig::Vector &  point_cloud  )

protected

Auxiliary function for computing cost function of the nonlinear problem to be solved with ipopt.

Parameters

x	is the variable
point	is one point of the point cloud

Returns

a part of the cost function value

Definition at line 202 of file superquadric.cpp.

 {
     double num1 = R(0, 0)*point_cloud[0] + R(0, 1)*point_cloud[1] + R(0, 2)*point_cloud[2] - x[5] * R(0, 0) - x[6] * R(0, 1) - x[7] * R(0, 2);
     double num2 = R(1, 0)*point_cloud[0] + R(1, 1)*point_cloud[1] + R(1, 2)*point_cloud[2] - x[5] * R(1, 0) - x[6] * R(1, 1) - x[7] * R(1, 2);
     double num3 = R(2, 0)*point_cloud[0] + R(2, 1)*point_cloud[1] + R(2, 2)*point_cloud[2] - x[5] * R(2, 0) - x[6] * R(2, 1) - x[7] * R(2, 2);
     double tmp = pow(abs(num1 / x[0]), 2.0 / x[4]) + pow(abs(num2 / x[1]), 2.0 / x[4]);
     return pow(abs(tmp), x[4] / x[3]) + pow(abs(num3 / x[2]), (2.0 / x[3]));
 }

finalize_solution()

void SuperQuadric_NLP::finalize_solution ( Ipopt::SolverReturn  status, Ipopt::Index  n, const Ipopt::Number *  x, const Ipopt::Number *  z_L, const Ipopt::Number *  z_U, Ipopt::Index  m, const Ipopt::Number *  g, const Ipopt::Number *  lambda, Ipopt::Number  obj_value, const Ipopt::IpoptData *  ip_data, Ipopt::IpoptCalculatedQuantities *  ip_cq  )

protected

Finalize the solution.

Parameters

n	is the dimension of the variable
x	is the variable
m	is the number of constraints
init_z	is an ipopt variable
z_L	is an ipopt variable
z_U	is an ipopt variable
status	says if the problem has been solved or not
obj_value	is the final cost function values

Definition at line 450 of file superquadric.cpp.

{
   solution.resize(n);
   for (Ipopt::Index i=0; i<n; i++)
       solution[i]=x[i];
}

get_bounds_info()

bool SuperQuadric_NLP::get_bounds_info ( Ipopt::Index  n, Ipopt::Number *  x_l, Ipopt::Number *  x_u, Ipopt::Index  m, Ipopt::Number *  g_l, Ipopt::Number *  g_u  )

protected

Get variable bounds for the nonlinear problem to be solved with ipopt.

Parameters

n	is the dimension of the variable
m	is the number of constraints
x_l	is the lower bound of the variable
x_u	is the upper bound of the variable
g_l	is the lower bound of the constraints
g_u	is the upper bound of the constraints

Returns

true

Definition at line 111 of file superquadric.cpp.

{
    computeBounds();

    for (Ipopt::Index i=0; i<n; i++)
    {
       x_l[i]=bounds(i,0);
       x_u[i]=bounds(i,1);
    }

    return true;
}

get_nlp_info()

bool SuperQuadric_NLP::get_nlp_info ( Ipopt::Index &  n, Ipopt::Index &  m, Ipopt::Index &  nnz_jac_g, Ipopt::Index &  nnz_h_lag, Ipopt::TNLP::IndexStyleEnum &  index_style  )

protected

Get info for the nonlinear problem to be solved with ipopt.

Parameters

n	is the dimension of the variable
m	is the number of constraints
nnz_jac_g	is the dimensions of the jacobian
nnz_h_lag	is an ipopt variable
index_styl	is an ipopt variable

Returns

true

Definition at line 70 of file superquadric.cpp.

{
    n=11;
    m=nnz_jac_g=nnz_h_lag=0;
    index_style=TNLP::C_STYLE;
    x_v.resize(n,0.0);

    return true;
}

get_result()

Vector SuperQuadric_NLP::get_result

(

)

const

Extract the solution.

Returns

the superquadric as a Vector

Definition at line 463 of file superquadric.cpp.

{
   return solution;
}

get_starting_point()

bool SuperQuadric_NLP::get_starting_point ( Ipopt::Index  n, bool  init_x, Ipopt::Number *  x, bool  init_z, Ipopt::Number *  z_L, Ipopt::Number *  z_U, Ipopt::Index  m, bool  init_lambda, Ipopt::Number *  lambda  )

protected

Get the starting point for the nonlinear problem to be solved with ipopt.

Parameters

n	is the dimension of the variable
init_x	is the starting point of the optimization problem
x	is the variable
init_z	is an ipopt variable
z_L	is an ipopt variable
z_U	is an ipopt variable
m	is the number of constraints
init_lambda	is an ipopt variable
lambda	is an ipopt variable

Returns

true

Definition at line 126 of file superquadric.cpp.

 {
     for (Ipopt::Index i=0;i<n;i++)
     {
         x[i]=x0[i];
     }
     return true;
 }

readMatrix()

bool SuperQuadric_NLP::readMatrix	(	const std::string &	tag,
		yarp::sig::Matrix &	matrix,
		const int &	dimension,
		yarp::os::ResourceFinder *	rf
	)

Function for reading matrices from config files.

Parameters

tag	is the name of the quantity to be read from text
matrix	is the matrix to be filled
dimensions	is the matrix dimensions
rf	is the resource finder

Returns

true/false on success/failure

Definition at line 388 of file superquadric.cpp.

{
   string tag_x=tag+"_x";
   string tag_y=tag+"_y";
   bool check_x;

   if (tag=="x0")
   {
       if (Bottle *b=rf->find(tag.c_str()).asList())
       {
           Vector col;
           if (b->size()>=dimension)
           {
               for(int i=0; i<b->size();i++)
                   col.push_back(b->get(i).asDouble());

               matrix.setCol(0, col);
           }
           return true;
       }
   }
   else
   {
       if (tag=="bounds_"+obj_class)
       {
           tag_x=tag+"_l";
           tag_y=tag+"_u";
       }

       if (Bottle *b=rf->find(tag_x.c_str()).asList())
       {
           Vector col;
           if (b->size()>=dimension)
           {
               for(int i=0; i<b->size();i++)
                   col.push_back(b->get(i).asDouble());

               matrix.setCol(0, col);
           }
           check_x=true;

       }

       if (Bottle *b=rf->find(tag_y.c_str()).asList())
       {
           Vector col;
           if (b->size()>=dimension)
           {
               for (int i=0; i<b->size();i++)
                   col.push_back(b->get(i).asDouble());
               matrix.setCol(1, col);
           }

           if (check_x==true)
               return true;
       }

   }
   return false;
}

setPoints()

void SuperQuadric_NLP::setPoints	(	const std::deque< yarp::sig::Vector > &	point_cloud,
		const int &	optimizer_points
	)

Set point to be used for superquadric estimation.

Parameters

point_cloud	is the object point cloud
optimizer_points	is the maximum number of points to be used for the optimization problem

Definition at line 44 of file superquadric.cpp.

{
    if (point_cloud.size()<optimizer_points)
    {
        for (size_t i=0;i<point_cloud.size();i++)
        {
            points_downsampled.push_back(point_cloud[i].subVector(0,2));
        }
    }
    else
    {
        int count=point_cloud.size()/optimizer_points;

        for (size_t i=0; i<point_cloud.size(); i+=count)
        {
            points_downsampled.push_back(point_cloud[i].subVector(0,2));
        }
    }

    yInfo("[Superquadric]: points actually used for modeling: %lu ",points_downsampled.size());

    x0.resize(11,0.0);
    computeX0(x0, points_downsampled);
}

---

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

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