Skip to content

Instantly share code, notes, and snippets.

@pattacini

pattacini/!test-superquadric.md

Last active October 15, 2018 11:28
Show Gist options

  • Select an option

    Learn more about clone URLs
  • Save pattacini/4eb6c5e40667d1242bc3470253d765af to your computer and use it in GitHub Desktop.

Select an option

Learn more about clone URLs
Save pattacini/4eb6c5e40667d1242bc3470253d765af to your computer and use it in GitHub Desktop.
Download ZIP
test-superquadric
Raw
!test-superquadric.md

test-superquadric

This code is derived from find-superquadric with the goal of quickly comparing the use of the finite differences Jacobian as implemented in superquadric-model against the analytical Jacobian in the computation of superquadrics.

Dependencies

How to run the test

  • launch superquadric-model --from config-classes.ini
  • launch test-superquadric --remove-outliers "(0.01 10)" --random-sample 0.2 --file path-to-file

The file config-classes.ini is contained in this gist with the correct options. Thus, it needs to be installed under ~/.local/share/yarp/contexts/superquadric-model.

The option --file requires passing the path to the file containing the point cloud as per the instructions than can be found in the find-superquadric repository.

Raw
CMakeLists.txt
# Copyright (C) 2018 Fondazione Istituto Italiano di Tecnologia (IIT)
# All Rights Reserved.
# Authors: Ugo Pattacini <[email protected]>
cmake_minimum_required(VERSION 3.5)
project(test-superquadric)
set(CMAKE_CXX_EXTENSIONS OFF)
set(CMAKE_CXX_STANDARD 11)
set(CMAKE_CXX_STANDARD_REQUIRED 11)
if(NOT CMAKE_CONFIGURATION_TYPES)
if(NOT CMAKE_BUILD_TYPE)
message(STATUS "Setting build type to 'Release' as none was specified.")
set_property(CACHE CMAKE_BUILD_TYPE PROPERTY VALUE "Release")
endif()
endif()
find_package(YARP 3.0.0 REQUIRED)
find_package(ICUB REQUIRED)
list(APPEND CMAKE_MODULE_PATH ${ICUB_MODULE_PATH})
find_package(IPOPT REQUIRED)
find_package(VTK REQUIRED)
include(${VTK_USE_FILE})
include_directories(${PROJECT_SOURCE_DIR} ${IPOPT_INCLUDE_DIRS})
add_definitions(${IPOPT_DEFINITIONS} -D_USE_MATH_DEFINES)
add_executable(${PROJECT_NAME} nlp.h nlp.cpp main.cpp)
set_property(TARGET ${PROJECT_NAME} APPEND_STRING PROPERTY LINK_FLAGS " ${IPOPT_LINK_FLAGS}")
target_link_libraries(${PROJECT_NAME} ${YARP_LIBRARIES} ${IPOPT_LIBRARIES} ${VTK_LIBRARIES} ctrlLib)
install(TARGETS ${PROJECT_NAME} DESTINATION bin)
Raw
config-classes.ini
# Ipopt parameters
mu_strategy adaptive
nlp_scaling_method gradient-based
tol 0.00001
optimizer_points 1000
bounds_automatic yes
bounds_default_u ( 0.5, 0.5, 0.5, 1.9, 1.9, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0)
bounds_default_l (0.02, 0.02, 0.02, 0.1, 0.1, -1.0, -1.0, -1.0, 0.0, 0.0, 0.0)
bounds_box_u ( 0.5, 0.5, 0.5, 1.0, 1.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0)
bounds_box_l (0.02, 0.02, 0.02, 0.1, 0.1, -1.0, -1.0, -1.0, 0.0, 0.0, 0.0)
bounds_cylinder_u ( 0.5, 0.5, 0.5, 0.5, 1.9, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0)
bounds_cylinder_l (0.02, 0.02, 0.02, 0.1, 0.1, -1.0, -1.0, -1.0, 0.0, 0.0, 0.0)
bounds_sphere_u ( 0.5, 0.5, 0.5, 1.5, 1.5, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0)
bounds_sphere_l (0.02, 0.02, 0.02, 0.5, 0.5, -1.0, -1.0, -1.0, 0.0, 0.0, 0.0)
# Visualization parameters
visualization_on off
color (255, 0, 0)
rate_vis 33
# Point cloud filter parameters
filter_points off
radius 0.005
nn-threshold 100
# Superq filter parameters
filter_superq off
fixed_window off
max_median_order 5
median_order 5
threshold_median 0.1
# Configuration parameters
object_class default
save_points off
one_shot off
rate 100
Raw
main.cpp
/******************************************************************************
* *
* Copyright (C) 2018 Fondazione Istituto Italiano di Tecnologia (IIT) *
* All Rights Reserved. *
* *
******************************************************************************/
/**
* @file main.cpp
* @authors: Ugo Pattacini <[email protected]>
*/
#include <cstdlib>
#include <memory>
#include <cmath>
#include <vector>
#include <set>
#include <algorithm>
#include <string>
#include <sstream>
#include <fstream>
#include <yarp/os/all.h>
#include <yarp/sig/all.h>
#include <yarp/math/Math.h>
#include <yarp/math/Rand.h>
#include <iCub/ctrl/clustering.h>
#include <vtkSmartPointer.h>
#include <vtkProperty.h>
#include <vtkPolyDataMapper.h>
#include <vtkPointData.h>
#include <vtkSuperquadric.h>
#include <vtkUnsignedCharArray.h>
#include <vtkTransform.h>
#include <vtkSampleFunction.h>
#include <vtkContourFilter.h>
#include <vtkRadiusOutlierRemoval.h>
#include <vtkActor.h>
#include <vtkOrientationMarkerWidget.h>
#include <vtkAxesActor.h>
#include <vtkRenderWindow.h>
#include <vtkRenderWindowInteractor.h>
#include <vtkRenderer.h>
#include <vtkVertexGlyphFilter.h>
#include <vtkCamera.h>
#include <vtkInteractorStyleSwitch.h>
#include "nlp.h"
using namespace std;
using namespace yarp::os;
using namespace yarp::sig;
using namespace yarp::math;
using namespace iCub::ctrl;
/****************************************************************/
class Object
{
protected:
vtkSmartPointer<vtkPolyDataMapper> vtk_mapper;
vtkSmartPointer<vtkActor> vtk_actor;
public:
/****************************************************************/
vtkSmartPointer<vtkActor> &get_actor()
{
return vtk_actor;
}
};
/****************************************************************/
class Points : public Object
{
protected:
vtkSmartPointer<vtkPoints> vtk_points;
vtkSmartPointer<vtkUnsignedCharArray> vtk_colors;
vtkSmartPointer<vtkPolyData> vtk_polydata;
vtkSmartPointer<vtkVertexGlyphFilter> vtk_glyphFilter;
public:
/****************************************************************/
Points(const vector<Vector> &points, const int point_size)
{
vtk_points=vtkSmartPointer<vtkPoints>::New();
for (size_t i=0; i<points.size(); i++)
vtk_points->InsertNextPoint(points[i][0],points[i][1],points[i][2]);
vtk_polydata=vtkSmartPointer<vtkPolyData>::New();
vtk_polydata->SetPoints(vtk_points);
vtk_glyphFilter=vtkSmartPointer<vtkVertexGlyphFilter>::New();
vtk_glyphFilter->SetInputData(vtk_polydata);
vtk_glyphFilter->Update();
vtk_mapper=vtkSmartPointer<vtkPolyDataMapper>::New();
vtk_mapper->SetInputConnection(vtk_glyphFilter->GetOutputPort());
vtk_actor=vtkSmartPointer<vtkActor>::New();
vtk_actor->SetMapper(vtk_mapper);
vtk_actor->GetProperty()->SetPointSize(point_size);
}
/****************************************************************/
void set_points(const vector<Vector> &points)
{
vtk_points=vtkSmartPointer<vtkPoints>::New();
for (size_t i=0; i<points.size(); i++)
vtk_points->InsertNextPoint(points[i][0],points[i][1],points[i][2]);
vtk_polydata->SetPoints(vtk_points);
}
/****************************************************************/
bool set_colors(const vector<vector<unsigned char>> &colors)
{
if (colors.size()==vtk_points->GetNumberOfPoints())
{
vtk_colors=vtkSmartPointer<vtkUnsignedCharArray>::New();
vtk_colors->SetNumberOfComponents(3);
for (size_t i=0; i<colors.size(); i++)
vtk_colors->InsertNextTypedTuple(colors[i].data());
vtk_polydata->GetPointData()->SetScalars(vtk_colors);
return true;
}
else
return false;
}
/****************************************************************/
vtkSmartPointer<vtkPolyData> &get_polydata()
{
return vtk_polydata;
}
};
/****************************************************************/
class Superquadric : public Object
{
protected:
vtkSmartPointer<vtkSuperquadric> vtk_superquadric;
vtkSmartPointer<vtkSampleFunction> vtk_sample;
vtkSmartPointer<vtkContourFilter> vtk_contours;
vtkSmartPointer<vtkTransform> vtk_transform;
public:
/****************************************************************/
Superquadric(const Vector &r, const vector<double> &color)
{
double bx=2.0*r[7];
double by=2.0*r[8];
double bz=2.0*r[9];
vtk_superquadric=vtkSmartPointer<vtkSuperquadric>::New();
vtk_superquadric->ToroidalOff();
vtk_superquadric->SetSize(1.0);
vtk_superquadric->SetCenter(zeros(3).data());
vtk_superquadric->SetScale(r[7],r[8],r[9]);
vtk_superquadric->SetPhiRoundness(r[10]);
vtk_superquadric->SetThetaRoundness(r[11]);
vtk_sample=vtkSmartPointer<vtkSampleFunction>::New();
vtk_sample->SetSampleDimensions(50,50,50);
vtk_sample->SetImplicitFunction(vtk_superquadric);
vtk_sample->SetModelBounds(-bx,bx,-by,by,-bz,bz);
// The isosurface is defined at 0.0 as specified in
// https://github.com/Kitware/VTK/blob/master/Common/DataModel/vtkSuperquadric.cxx
vtk_contours=vtkSmartPointer<vtkContourFilter>::New();
vtk_contours->SetInputConnection(vtk_sample->GetOutputPort());
vtk_contours->GenerateValues(1,0.0,0.0);
vtk_mapper=vtkSmartPointer<vtkPolyDataMapper>::New();
vtk_mapper->SetInputConnection(vtk_contours->GetOutputPort());
vtk_mapper->ScalarVisibilityOff();
vtk_actor=vtkSmartPointer<vtkActor>::New();
vtk_actor->SetMapper(vtk_mapper);
vtk_actor->GetProperty()->SetColor(color[0],color[1],color[2]);
vtk_actor->GetProperty()->SetOpacity(0.25);
vtk_transform=vtkSmartPointer<vtkTransform>::New();
vtk_transform->Translate(r.subVector(0,2).data());
vtk_transform->RotateWXYZ((180.0/M_PI)*r[6],r.subVector(3,5).data());
vtk_actor->SetUserTransform(vtk_transform);
}
};
/****************************************************************/
class Finder : public RFModule
{
Bottle outliersRemovalOptions;
unsigned int uniform_sample;
double random_sample;
bool test_derivative;
double inside_penalty;
vector<Vector> all_points,in_points,out_points,dwn_points;
vector<vector<unsigned char>> all_colors;
unique_ptr<Points> vtk_all_points,vtk_out_points,vtk_dwn_points;
RpcClient port;
/****************************************************************/
void removeOutliers()
{
double t0=Time::now();
if (outliersRemovalOptions.size()>=2)
{
double radius=outliersRemovalOptions.get(0).asDouble();
int minpts=outliersRemovalOptions.get(1).asInt();
Property options;
options.put("epsilon",radius);
options.put("minpts",minpts);
DBSCAN dbscan;
map<size_t,set<size_t>> clusters=dbscan.cluster(all_points,options);
size_t largest_class; size_t largest_size=0;
for (auto it=begin(clusters); it!=end(clusters); it++)
{
if (it->second.size()>largest_size)
{
largest_size=it->second.size();
largest_class=it->first;
}
}
auto &c=clusters[largest_class];
for (size_t i=0; i<all_points.size(); i++)
{
if (c.find(i)==end(c))
out_points.push_back(all_points[i]);
else
in_points.push_back(all_points[i]);
}
}
else
in_points=all_points;
double t1=Time::now();
yInfo()<<out_points.size()<<"outliers removed out of"
<<all_points.size()<<"points in"<<t1-t0<<"[s]";
}
/****************************************************************/
void sampleInliers()
{
double t0=Time::now();
if (random_sample>=1.0)
{
unsigned int cnt=0;
for (auto &p:in_points)
{
if ((cnt++%uniform_sample)==0)
dwn_points.push_back(p);
}
}
else
{
set<unsigned int> idx;
while (idx.size()<(size_t)(random_sample*in_points.size()))
{
unsigned int i=(unsigned int)(Rand::scalar(0.0,1.0)*in_points.size());
if (idx.find(i)==idx.end())
{
dwn_points.push_back(in_points[i]);
idx.insert(i);
}
}
}
double t1=Time::now();
yInfo()<<dwn_points.size()<<"samples out of"
<<in_points.size()<<"inliers in"<<t1-t0<<"[s]";
}
/****************************************************************/
Vector findSuperquadric() const
{
Ipopt::SmartPtr<Ipopt::IpoptApplication> app=new Ipopt::IpoptApplication;
app->Options()->SetNumericValue("tol",1e-6);
app->Options()->SetNumericValue("constr_viol_tol",1e-3);
app->Options()->SetIntegerValue("acceptable_iter",0);
app->Options()->SetStringValue("mu_strategy","adaptive");
app->Options()->SetIntegerValue("max_iter",1000);
app->Options()->SetStringValue("hessian_approximation","limited-memory");
app->Options()->SetStringValue("derivative_test",test_derivative?"first-order":"none");
app->Options()->SetIntegerValue("print_level",test_derivative?5:0);
app->Initialize();
double t0=Time::now();
Ipopt::SmartPtr<SuperQuadricNLP> nlp=new SuperQuadricNLP(dwn_points,inside_penalty);
Ipopt::ApplicationReturnStatus status=app->OptimizeTNLP(GetRawPtr(nlp));
double t1=Time::now();
Vector r=nlp->get_result();
yInfo()<<"center = ("<<r.subVector(0,2).toString(3,3)<<")";
yInfo()<<"angle ="<<r[3]<<"[deg]";
yInfo()<<"size = ("<<r.subVector(4,6).toString(3,3)<<")";
yInfo()<<"shape = ("<<r.subVector(7,8).toString(3,3)<<")";
yInfo()<<"found in ="<<t1-t0<<"[s]";
Vector rot(4,0.0);
rot[2]=1.0; rot[3]=(M_PI/180.0)*r[3];
Vector r_=r.subVector(0,2);
r_=cat(r_,rot);
r_=cat(r_,r.subVector(4,6));
r_=cat(r_,r.subVector(7,8));
return r_;
}
/****************************************************************/
bool configure(ResourceFinder &rf) override
{
Rand::init();
if (rf.check("file"))
{
string file=rf.find("file").asString();
ifstream fin(file.c_str());
if (!fin.is_open())
{
yError()<<"Unable to open file \""<<file<<"\"";
return false;
}
Vector p(3);
vector<unsigned int> c_(3);
vector<unsigned char> c(3);
string line;
while (getline(fin,line))
{
istringstream iss(line);
if (!(iss>>p[0]>>p[1]>>p[2]))
break;
all_points.push_back(p);
fill(c_.begin(),c_.end(),120);
iss>>c_[0]>>c_[1]>>c_[2];
c[0]=(unsigned char)c_[0];
c[1]=(unsigned char)c_[1];
c[2]=(unsigned char)c_[2];
all_colors.push_back(c);
}
}
else
{
yError()<<"Unable to find command-line option \"--file\"";
return false;
}
port.open("/test-superquadric:rpc");
if (!Network::connect(port.getName(),"/superquadric-model/rpc"))
{
yError()<<"Unable to connect to superquadric-model";
close();
return false;
}
if (rf.check("remove-outliers"))
if (const Bottle *ptr=rf.find("remove-outliers").asList())
outliersRemovalOptions=*ptr;
uniform_sample=(unsigned int)rf.check("uniform-sample",Value(1)).asInt();
random_sample=rf.check("random-sample",Value(1.0)).asDouble();
inside_penalty=rf.check("inside-penalty",Value(1.0)).asDouble();
test_derivative=rf.check("test-derivative");
removeOutliers();
sampleInliers();
vtk_all_points=unique_ptr<Points>(new Points(all_points,2));
vtk_out_points=unique_ptr<Points>(new Points(out_points,4));
vtk_dwn_points=unique_ptr<Points>(new Points(dwn_points,1));
vtk_all_points->set_colors(all_colors);
vtk_out_points->get_actor()->GetProperty()->SetColor(1.0,0.0,0.0);
vtk_dwn_points->get_actor()->GetProperty()->SetColor(1.0,1.0,0.0);
Bottle cmd,rep;
cmd.addString("send_point_clouds");
Bottle &payLoad=cmd.addList();
for (auto &p:dwn_points)
payLoad.addList().read(cat(p,zeros(3)));
port.write(cmd,rep);
cmd.clear();
cmd.addString("get_superq");
port.write(cmd,rep);
Property prop; prop.fromString(rep.get(0).asList()->toString());
yInfo()<<"received options:"<<prop.toString();
Vector r_finitediff,v;
prop.find("center").asList()->write(v); r_finitediff=cat(r_finitediff,v);
prop.find("orientation").asList()->write(v); r_finitediff=cat(r_finitediff,v);
prop.find("dimensions").asList()->write(v); r_finitediff=cat(r_finitediff,v);
prop.find("exponents").asList()->write(v); r_finitediff=cat(r_finitediff,v);
yInfo()<<"final superquadric_finitediff parameters:"<<r_finitediff.toString(3,3);
unique_ptr<Superquadric> vtk_superquadric_finitediff=unique_ptr<Superquadric>(new Superquadric(r_finitediff,vector<double>{0.0,1.0,0.0}));
Vector r_analytic=findSuperquadric();
unique_ptr<Superquadric> vtk_superquadric_analytic=unique_ptr<Superquadric>(new Superquadric(r_analytic,vector<double>{0.0,0.0,1.0}));
vtkSmartPointer<vtkRenderer> vtk_renderer=vtkSmartPointer<vtkRenderer>::New();
vtkSmartPointer<vtkRenderWindow> vtk_renderWindow=vtkSmartPointer<vtkRenderWindow>::New();
vtk_renderWindow->SetSize(300,300);
vtk_renderWindow->AddRenderer(vtk_renderer);
vtkSmartPointer<vtkRenderWindowInteractor> vtk_renderWindowInteractor=vtkSmartPointer<vtkRenderWindowInteractor>::New();
vtk_renderWindowInteractor->SetRenderWindow(vtk_renderWindow);
vtk_renderer->AddActor(vtk_all_points->get_actor());
vtk_renderer->AddActor(vtk_out_points->get_actor());
if (dwn_points.size()!=in_points.size())
vtk_renderer->AddActor(vtk_dwn_points->get_actor());
vtk_renderer->AddActor(vtk_superquadric_analytic->get_actor());
vtk_renderer->AddActor(vtk_superquadric_finitediff->get_actor());
vtk_renderer->SetBackground(0.1,0.2,0.2);
vtkSmartPointer<vtkAxesActor> vtk_axes=vtkSmartPointer<vtkAxesActor>::New();
vtkSmartPointer<vtkOrientationMarkerWidget> vtk_widget=vtkSmartPointer<vtkOrientationMarkerWidget>::New();
vtk_widget->SetOutlineColor(0.9300,0.5700,0.1300);
vtk_widget->SetOrientationMarker(vtk_axes);
vtk_widget->SetInteractor(vtk_renderWindowInteractor);
vtk_widget->SetViewport(0.0,0.0,0.2,0.2);
vtk_widget->SetEnabled(1);
vtk_widget->InteractiveOn();
vector<double> bounds(6),centroid(3);
vtk_all_points->get_polydata()->GetBounds(bounds.data());
for (size_t i=0; i<centroid.size(); i++)
centroid[i]=0.5*(bounds[i<<1]+bounds[(i<<1)+1]);
vtkSmartPointer<vtkCamera> vtk_camera=vtkSmartPointer<vtkCamera>::New();
vtk_camera->SetPosition(centroid[0]+1.0,centroid[1],centroid[2]+0.5);
vtk_camera->SetFocalPoint(centroid.data());
vtk_camera->SetViewUp(0.0,0.0,1.0);
vtk_renderer->SetActiveCamera(vtk_camera);
vtkSmartPointer<vtkInteractorStyleSwitch> vtk_style=vtkSmartPointer<vtkInteractorStyleSwitch>::New();
vtk_style->SetCurrentStyleToTrackballCamera();
vtk_renderWindowInteractor->SetInteractorStyle(vtk_style);
vtk_renderWindowInteractor->Start();
return true;
}
/****************************************************************/
bool updateModule() override
{
return false;
}
/****************************************************************/
bool close() override
{
port.close();
return true;
}
};
/****************************************************************/
int main(int argc, char *argv[])
{
Network yarp;
if (!yarp.checkNetwork())
{
yError()<<"Unable to find Yarp server!";
return EXIT_FAILURE;
}
ResourceFinder rf;
rf.configure(argc,argv);
Finder finder;
return finder.runModule(rf);
}
Raw
nlp.cpp
/******************************************************************************
* *
* Copyright (C) 2018 Fondazione Istituto Italiano di Tecnologia (IIT) *
* All Rights Reserved. *
* *
******************************************************************************/
/**
* @file nlp.cpp
* @authors: Ugo Pattacini <[email protected]>
*/
#include <cmath>
#include <limits>
#include <yarp/math/Math.h>
#include "nlp.h"
using namespace std;
using namespace yarp::sig;
using namespace yarp::math;
/****************************************************************/
bool SuperQuadricNLP::get_nlp_info(Ipopt::Index &n, Ipopt::Index &m,
Ipopt::Index &nnz_jac_g,
Ipopt::Index &nnz_h_lag,
IndexStyleEnum &index_style)
{
n=9; m=1;
nnz_jac_g=2;
nnz_h_lag=0;
index_style=TNLP::C_STYLE;
return true;
}
/****************************************************************/
bool SuperQuadricNLP::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)
{
Vector margin(3);
margin[0]=0.25*(bounds(0,1)-bounds(0,0));
margin[1]=0.25*(bounds(1,1)-bounds(1,0));
margin[2]=0.25*(bounds(2,1)-bounds(2,0));
// center
x_l[0]=bounds(0,0)+margin[0]; x_u[0]=bounds(0,1)-margin[0];
x_l[1]=bounds(1,0)+margin[1]; x_u[1]=bounds(1,1)-margin[1];
x_l[2]=bounds(2,0)+margin[2]; x_u[2]=bounds(2,1)-margin[2];
// angle around z-axis
x_l[3]=0.0; x_u[3]=2.0*M_PI;
// size
x_l[4]=0.001; x_u[4]=numeric_limits<double>::infinity();
x_l[5]=0.001; x_u[5]=numeric_limits<double>::infinity();
x_l[6]=0.001; x_u[6]=numeric_limits<double>::infinity();
// shape
x_l[7]=0.1; x_u[7]=1.0;
x_l[8]=0.1; x_u[8]=1.0;
// limit on z-min
g_l[0]=bounds(2,0); g_u[0]=numeric_limits<double>::infinity();
return true;
}
/****************************************************************/
bool SuperQuadricNLP::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)
{
x[0]=centroid[0];
x[1]=centroid[1];
x[2]=centroid[2];
x[3]=0.0;
x[4]=0.5*(bounds(0,1)-bounds(0,0));
x[5]=0.5*(bounds(1,1)-bounds(1,0));
x[6]=0.5*(bounds(2,1)-bounds(2,0));
x[7]=1.0;
x[8]=1.0;
return true;
}
/****************************************************************/
bool SuperQuadricNLP::eval_f(Ipopt::Index n, const Ipopt::Number *x,
bool new_x, Ipopt::Number &obj_value)
{
Vector c(3),s(3);
c[0]=x[0];
c[1]=x[1];
c[2]=x[2];
const double &a=x[3];
s[0]=x[4];
s[1]=x[5];
s[2]=x[6];
const double &e1=x[7];
const double &e2=x[8];
Vector rot(4,0.0); rot[2]=1.0; rot[3]=a;
Matrix T=axis2dcm(rot);
T.setSubcol(c,0,3);
T=SE3inv(T);
obj_value=0.0;
Vector p1(4,1.0);
for (auto &p:points)
{
p1.setSubvector(0,p);
p1=T*p1;
double tx=pow(abs(p1[0]/s[0]),2.0/e2);
double ty=pow(abs(p1[1]/s[1]),2.0/e2);
double tz=pow(abs(p1[2]/s[2]),2.0/e1);
double F1=pow(pow(tx+ty,e2/e1)+tz,e1)-1.0;
double penalty=(F1<0.0?inside_penalty:1.0);
obj_value+=F1*F1*penalty;
}
obj_value*=(s[0]*s[1]*s[2])/points.size();
return true;
}
/****************************************************************/
bool SuperQuadricNLP::eval_grad_f(Ipopt::Index n, const Ipopt::Number *x,
bool new_x, Ipopt::Number *grad_f)
{
Vector c(3),s(3);
c[0]=x[0];
c[1]=x[1];
c[2]=x[2];
const double &a=x[3];
s[0]=x[4];
s[1]=x[5];
s[2]=x[6];
const double &e1=x[7];
const double &e2=x[8];
Vector rot(4,0.0); rot[2]=1.0; rot[3]=a;
Matrix T=axis2dcm(rot);
T.setSubcol(c,0,3);
T=SE3inv(T);
for (Ipopt::Index i=0; i<n; i++)
grad_f[i]=0.0;
double coeff=s[0]*s[1]*s[2];
Vector p1(4,1.0);
for (auto &p:points)
{
p1.setSubvector(0,p);
p1=T*p1;
double tx=pow(abs(p1[0]/s[0]),2.0/e2);
double ty=pow(abs(p1[1]/s[1]),2.0/e2);
double tz=pow(abs(p1[2]/s[2]),2.0/e1);
double F1=pow(pow(tx+ty,e2/e1)+tz,e1)-1.0;
double penalty=(F1<0.0?inside_penalty:1.0);
double tmp1=2.0*coeff*F1*penalty;
double tmp2=F1*F1*penalty;
double t11=cos(a)*(c[0]-p[0])+sin(a)*(c[1]-p[1]);
double t10=cos(a)*(c[1]-p[1])-sin(a)*(c[0]-p[0]);
double t9=pow(abs(t11)/s[0],2.0/e2);
double t8=pow(abs(t10)/s[1],2.0/e2);
double t7=abs(c[2]-p[2])/s[2];
double t6=pow(t9+t8,e2/e1);
double t5=pow(t7,2.0/e1-1.0);
double t4=pow(abs(t11)/s[0],2.0/e2-1.0);
double t3=pow(abs(t10)/s[1],2.0/e2-1.0);
double t2=pow(t7,2.0/e1)+t6;
double t1=pow(t9+t8,e2/e1-1.0);
double t0=pow(t2,e1-1.0);
grad_f[0]+=tmp1 * (-t1*t0*2.0*((sign(t10)*sin(a)*t3)/s[1]-(sign(t11)*cos(a)*t4)/s[0]));
grad_f[1]+=tmp1 * (t1*t0*2.0*((sign(t11)*sin(a)*t4)/s[0]+(sign(t10)*cos(a)*t3)/s[1]));
grad_f[2]+=tmp1 * ((sign(c[2]-p[2])*t0*t5*2.0)/s[2]);
grad_f[3]+=tmp1 * (2.0*t1*t0*((sign(t11)*t4*t10)/s[0]-(sign(t10)*t3*t11)/s[1]));
grad_f[4]+=tmp1 * (-(abs(t11)*t0*t4*t1*2.0)/(s[0]*s[0])) + tmp2 * s[1]*s[2];
grad_f[5]+=tmp1 * (-(abs(t10)*t0*t3*t1*2.0)/(s[1]*s[1])) + tmp2 * s[0]*s[2];
grad_f[6]+=tmp1 * (-(abs(c[2]-p[2])*t0*t5*2.0)/(s[2]*s[2])) + tmp2 * s[0]*s[1];
grad_f[7]+=tmp1 * ((log(t2)*pow(t2,e1)-t0*(log(t7)*pow(t7,2.0/e1)*2.0+e2*log(t9+t8)*t6)/e1));
grad_f[8]+=tmp1 * (t0*(log(t9+t8)*t6-2.0*t1*(log(abs(t11)/s[0])*t9+log(abs(t10)/s[1])*t8)/e2));
}
for (Ipopt::Index i=0; i<n; i++)
grad_f[i]/=points.size();
return true;
}
/****************************************************************/
bool SuperQuadricNLP::eval_g(Ipopt::Index n, const Ipopt::Number *x,
bool new_x, Ipopt::Index m, Ipopt::Number *g)
{
g[0]=x[2]-x[6];
return true;
}
/****************************************************************/
bool SuperQuadricNLP::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)
{
if (values==nullptr)
{
iRow[0]=0; jCol[0]=2;
iRow[1]=0; jCol[1]=6;
}
else
{
values[0]=1.0;
values[1]=-1.0;
}
return true;
}
/****************************************************************/
bool SuperQuadricNLP::eval_h(Ipopt::Index n, const Ipopt::Number *x,
bool new_x, Ipopt::Number obj_factor,
Ipopt::Index m, const Ipopt::Number *lambda,
bool new_lambda, Ipopt::Index nele_hess,
Ipopt::Index *iRow, Ipopt::Index *jCol,
Ipopt::Number *values)
{
return true;
}
/****************************************************************/
void SuperQuadricNLP::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)
{
result.resize(n);
for (Ipopt::Index i=0; i<n; i++)
result[i]=x[i];
result[3]*=180.0/M_PI;
}
/****************************************************************/
SuperQuadricNLP::SuperQuadricNLP(const vector<Vector> &points_,
const double inside_penalty_) :
points(points_), inside_penalty(inside_penalty_)
{
bounds.resize(3,2);
bounds(0,0)=bounds(1,0)=bounds(2,0)=numeric_limits<double>::infinity();
bounds(0,1)=bounds(1,1)=bounds(2,1)=-numeric_limits<double>::infinity();
for (auto &p:points)
{
if (p[0]<bounds(0,0))
bounds(0,0)=p[0];
if (p[0]>bounds(0,1))
bounds(0,1)=p[0];
if (p[1]<bounds(1,0))
bounds(1,0)=p[1];
if (p[1]>bounds(1,1))
bounds(1,1)=p[1];
if (p[2]<bounds(2,0))
bounds(2,0)=p[2];
if (p[2]>bounds(2,1))
bounds(2,1)=p[2];
}
centroid.resize(3,0.0);
for (unsigned int i=0; i<centroid.length(); i++)
centroid[i]=0.5*(bounds(i,0)+bounds(i,1));
}
/****************************************************************/
Vector SuperQuadricNLP::get_result() const
{
return result;
}
Raw
nlp.h
/******************************************************************************
* *
* Copyright (C) 2018 Fondazione Istituto Italiano di Tecnologia (IIT) *
* All Rights Reserved. *
* *
******************************************************************************/
/**
* @file nlp.h
* @authors: Ugo Pattacini <[email protected]>
*/
#ifndef NLP_H
#define NLP_H
#include <vector>
#include <IpTNLP.hpp>
#include <IpIpoptApplication.hpp>
#include <yarp/sig/Vector.h>
#include <yarp/sig/Matrix.h>
/****************************************************************/
class SuperQuadricNLP : public Ipopt::TNLP
{
protected:
const std::vector<yarp::sig::Vector> &points;
double inside_penalty;
yarp::sig::Vector centroid;
yarp::sig::Matrix bounds;
yarp::sig::Vector result;
/****************************************************************/
bool get_nlp_info(Ipopt::Index &n, Ipopt::Index &m, Ipopt::Index &nnz_jac_g,
Ipopt::Index &nnz_h_lag, IndexStyleEnum &index_style) override;
/****************************************************************/
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) override;
/****************************************************************/
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) override;
/****************************************************************/
bool eval_f(Ipopt::Index n, const Ipopt::Number *x, bool new_x,
Ipopt::Number &obj_value) override;
/****************************************************************/
bool eval_grad_f(Ipopt::Index n, const Ipopt::Number *x, bool new_x,
Ipopt::Number *grad_f) override;
/****************************************************************/
bool eval_g(Ipopt::Index n, const Ipopt::Number *x, bool new_x,
Ipopt::Index m, Ipopt::Number *g) override;
/****************************************************************/
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) override;
/****************************************************************/
bool eval_h(Ipopt::Index n, const Ipopt::Number *x, bool new_x,
Ipopt::Number obj_factor, Ipopt::Index m, const Ipopt::Number *lambda,
bool new_lambda, Ipopt::Index nele_hess, Ipopt::Index *iRow,
Ipopt::Index *jCol, Ipopt::Number *values) override;
/****************************************************************/
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) override;
public:
/****************************************************************/
SuperQuadricNLP(const std::vector<yarp::sig::Vector> &points_,
const double inside_penalty_);
/****************************************************************/
yarp::sig::Vector get_result() const;
};
#endif
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment