GTEST_TEST(TestConvexHull, testRandomConvexCombinations) {
// Generate a set of points, then find a random convex combination of those
// points, and verify that it's correctly reported as being inside the
// convex hull
for (int i = 2; i < 50; ++i) {
for (int j = 0; j < 500; ++j) {
MatrixXd pts = MatrixXd::Random(2, i);
VectorXd weights = VectorXd::Random(i);
if (weights.minCoeff() < 0) {
weights = weights.array() -
weights.minCoeff(); // make sure they're all nonnegative
}
weights = weights.array() / weights.sum();
Vector2d q = pts * weights;
EXPECT_TRUE(inConvexHull(pts, q, 1e-8));
}
}
}
Matrix<bool, Dynamic, 1> getActiveSupportMask(
const RigidBodyTree &r, VectorXd q, VectorXd qd,
std::vector<SupportStateElement,
Eigen::aligned_allocator<SupportStateElement>> &
available_supports,
const Ref<const Matrix<bool, Dynamic, 1>> &contact_force_detected,
double contact_threshold) {
KinematicsCache<double> cache = r.doKinematics(q, qd);
size_t nsupp = available_supports.size();
Matrix<bool, Dynamic, 1> active_supp_mask =
Matrix<bool, Dynamic, 1>::Zero(nsupp);
VectorXd phi;
SupportStateElement se;
bool needs_kin_check;
bool kin_contact;
bool force_contact;
for (size_t i = 0; i < nsupp; i++) {
// mexPrintf("evaluating support: %d\n", i);
se = available_supports[i];
force_contact = (contact_force_detected(se.body_idx) != 0);
// Determine if the body needs to be checked for kinematic contact. We only
// need to check for kin contact if the logic map indicates that the
// presence or absence of such contact would affect the decision about
// whether to use that body as a support.
needs_kin_check = (((se.support_logic_map[1] != se.support_logic_map[0]) &&
(contact_force_detected(se.body_idx) == 0)) ||
((se.support_logic_map[3] != se.support_logic_map[2]) &&
(contact_force_detected(se.body_idx) == 1)));
if (needs_kin_check) {
if (contact_threshold == -1) {
kin_contact = true;
} else {
contactPhi(r, cache, se, phi);
kin_contact = (phi.minCoeff() <= contact_threshold);
}
} else {
kin_contact = false; // we've determined already that kin contact doesn't
// matter for this support element
}
active_supp_mask(i) =
isSupportElementActive(&se, force_contact, kin_contact);
// mexPrintf("needs check: %d force contact: %d kin_contact: %d is_active:
// %d\n", needs_kin_check, force_contact, kin_contact, active_supp_mask(i));
}
return active_supp_mask;
}
void UpdaterMean::costsToWeights(const VectorXd& costs, string weighting_method, double eliteness, VectorXd& weights) const
{
weights.resize(costs.size());
if (weighting_method.compare("PI-BB")==0)
{
// PI^2 style weighting: continuous, cost exponention
double h = eliteness; // In PI^2, eliteness parameter is known as "h"
double range = costs.maxCoeff()-costs.minCoeff();
if (range==0)
weights.fill(1);
else
weights = (-h*(costs.array()-costs.minCoeff())/range).exp();
}
else if (weighting_method.compare("CMA-ES")==0 || weighting_method.compare("CEM")==0 )
{
// CMA-ES and CEM are rank-based, so we must first sort the costs, and the assign a weight to
// each rank.
VectorXd costs_sorted = costs;
std::sort(costs_sorted.data(), costs_sorted.data()+costs_sorted.size());
// In Python this is more elegant because we have argsort.
// indices = np.argsort(costs)
// It is possible to do this with fancy lambda functions or std::pair in C++ too, but I don't
// mind writing two for loops instead ;-)
weights.fill(0.0);
int mu = eliteness; // In CMA-ES, eliteness parameter is known as "mu"
assert(mu<costs.size());
for (int ii=0; ii<mu; ii++)
{
double cur_cost = costs_sorted[ii];
for (int jj=0; jj<costs.size(); jj++)
{
if (costs[jj] == cur_cost)
{
if (weighting_method.compare("CEM")==0)
weights[jj] = 1.0/mu; // CEM
else
weights[jj] = log(mu+0.5) - log(ii+1); // CMA-ES
break;
}
}
}
// For debugging
//MatrixXd print_mat(3,costs.size());
//print_mat.row(0) = costs_sorted;
//print_mat.row(1) = costs;
//print_mat.row(2) = weights;
//cout << print_mat << endl;
}
else
{
cout << __FILE__ << ":" << __LINE__ << ":WARNING: Unknown weighting method '" << weighting_method << "'. Calling with PI-BB weighting." << endl;
costsToWeights(costs, "PI-BB", eliteness, weights);
return;
}
// Relative standard deviation of total costs
double mean = weights.mean();
double std = sqrt((weights.array()-mean).pow(2).mean());
double rel_std = std/mean;
if (rel_std<1e-10)
{
// Special case: all costs are the same
// Set same weights for all.
weights.fill(1);
}
// Normalize weights
weights = weights/weights.sum();
}
int main(int argc, char* argv[])
{
//Misc. initialization
StartTime = (unsigned)time(0); //Time the program starts
cout.precision(12);
cout << fixed;
//End of section
//Initialize local variables
fstream vcidata,spectfile; //File streams for the input and output files
MatrixXd VCIHam; //Full Hamiltonian matrix
MatrixXd CIVec; //CI eigenvectors (wavefunction)
VectorXd CIFreq; //CI eigenvalues (frequencies
double Ezpe = 0; //CI zero-point energy
double RunTime; //Run time for the calculations
string TimeUnits; //Seconds, minutes, or hours for RunTime
//End of section
//Print title and compile date
PrintFancyTitle();
cout << "Last modification: ";
cout << __TIME__ << " on ";
cout << __DATE__ << '\n';
cout << '\n';
cout.flush();
//End of section
//Gather input and check for errors
cout << "Reading input..." << '\n';
ReadCIArgs(argc,argv,vcidata,spectfile); //Read arguments
ReadCIInput(VCIHam,vcidata); //Read input files
//End of section
//Adjust force constants
ScaleFC();
//End of section
//Calculate spectrum
cout << "Constructing the CI Hamiltonian...";
cout << '\n';
cout << " Zeroth-order Hamiltionian";
cout.flush(); //Print progress
ZerothHam(VCIHam); //Harmonic terms
cout << "; Done." << '\n';
cout << " Adding anharmonic potential";
cout.flush(); //Print progress
AnharmHam(VCIHam); //Anharmonic terms
cout << "; Done." << '\n';
cout << '\n';
cout << "Diagonalizing the Hamiltonian...";
cout << '\n' << '\n';
cout.flush(); //Print progress
VCIDiagonalize(VCIHam,CIVec,CIFreq); //Diagonalization wrapper
//End of section
//Free memory
VCIHam = MatrixXd(1,1);
//End of section
//Calculate and remove ZPE
VectorXd UnitVec(BasisSet.size()); //Create a unit vector
UnitVec.setOnes(); //Subtracting a constant requires a vector
Ezpe = CIFreq.minCoeff(); //Find ZPE
if (Ezpe < 0)
{
//Print error
cout << "Error: The zero-point energy is negative.";
cout << " Something is wrong.";
cout << '\n';
cout << "Try reducing the number of quanta or removing";
cout << " large anharmonic";
cout << '\n';
cout << "force constants.";
cout << '\n' << '\n';
cout.flush();
//Quit
exit(0);
}
CIFreq -= (Ezpe*UnitVec); //Remove ZPE from all frequencies
//End of section
//Free memory
UnitVec = VectorXd(1);
//End of section
//Print results
cout << "Printing the spectrum...";
cout << '\n' << '\n';
cout << "Results:" << '\n';
cout << " Zero-point energy: ";
cout.precision(2); //Truncate energy
cout << Ezpe << " (1/cm)"; //Print energy
cout.precision(12); //Replace settings
cout << '\n';
cout.flush(); //Print progress
PrintSpectrum(CIFreq,CIVec,spectfile); //Convert the frequencies to a spectrum
EndTime = (unsigned)time(0); //Time the calculation stops
RunTime = (double)(EndTime-StartTime); //Total run time
if (RunTime >= 3600)
{
//Switch to hours
RunTime /= 3600;
TimeUnits = "hours";
}
else if (RunTime >= 60)
{
//Switch to minutes
RunTime /= 60;
TimeUnits = "minutes";
}
else
{
//Stick with seconds
TimeUnits = "seconds";
}
cout.precision(2); //Truncate time
cout << " Run time: " << RunTime;
cout.precision(12); //Replace settings
cout << " " << TimeUnits;
cout << '\n' << '\n';
cout << "Done.";
cout << '\n' << '\n';
cout.flush(); //Print progress
//End of section
//Empty memory and delete junk files
vcidata.close();
spectfile.close();
//End of section
//Quit
return 0;
};
void mexFunction(int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[])
{
// This is useful for debugging whether Matlab is caching the mex binary
//mexPrintf("%s %s\n",__TIME__,__DATE__);
igl::matlab::MexStream mout;
std::streambuf *outbuf = std::cout.rdbuf(&mout);
using namespace std;
using namespace Eigen;
using namespace igl;
using namespace igl::matlab;
using namespace igl::copyleft::cgal;
MatrixXd V;
MatrixXi F;
igl::copyleft::cgal::RemeshSelfIntersectionsParam params;
string prefix;
bool use_obj_format = false;
if(nrhs < 2)
{
mexErrMsgTxt("nrhs < 2");
}
parse_rhs_double(prhs,V);
parse_rhs_index(prhs+1,F);
mexErrMsgTxt(V.cols()==3,"V must be #V by 3");
mexErrMsgTxt(F.cols()==3,"F must be #F by 3");
if(nrhs>2)
{
int i = 2;
while(i<nrhs)
{
if(!mxIsChar(prhs[i]))
{
mexErrMsgTxt("Parameter names should be char strings");
}
// Cast to char
const char * name = mxArrayToString(prhs[i]);
if(strcmp("DetectOnly",name) == 0)
{
validate_arg_scalar(i,nrhs,prhs,name);
validate_arg_logical(i,nrhs,prhs,name);
mxLogical * v = (mxLogical *)mxGetData(prhs[++i]);
params.detect_only = *v;
}else if(strcmp("FirstOnly",name) == 0)
{
validate_arg_scalar(i,nrhs,prhs,name);
validate_arg_logical(i,nrhs,prhs,name);
mxLogical * v = (mxLogical *)mxGetData(prhs[++i]);
params.first_only = *v;
}else if(strcmp("StitchAll",name) == 0)
{
validate_arg_scalar(i,nrhs,prhs,name);
validate_arg_logical(i,nrhs,prhs,name);
mxLogical * v = (mxLogical *)mxGetData(prhs[++i]);
params.stitch_all = *v;
}else
{
mexErrMsgTxt(C_STR("Unsupported parameter: "<<name));
}
i++;
}
}
MatrixXi IF;
VectorXi J,IM;
if(F.rows()>0)
{
// Check that there aren't any combinatorially or geometrically degenerate triangles
VectorXd A;
doublearea(V,F,A);
if(A.minCoeff()<=0)
{
mexErrMsgTxt("Geometrically degenerate face found.");
}
if(
(F.array().col(0) == F.array().col(1)).any() ||
(F.array().col(1) == F.array().col(2)).any() ||
(F.array().col(2) == F.array().col(0)).any())
{
mexErrMsgTxt("Combinatorially degenerate face found.");
}
// Now mesh self intersections
{
MatrixXd tempV;
MatrixXi tempF;
remesh_self_intersections(V,F,params,tempV,tempF,IF,J,IM);
//cout<<BLUEGIN("Found and meshed "<<IF.rows()<<" pair"<<(IF.rows()==1?"":"s")
// <<" of self-intersecting triangles.")<<endl;
V=tempV;
F=tempF;
}
// Double-check output
#ifdef DEBUG
// There should be *no* combinatorial duplicates
{
MatrixXi tempF;
unique_simplices(F,tempF);
if(tempF.rows() < F.rows())
{
cout<<REDRUM("Error: selfintersect created "<<
F.rows()-tempF.rows()<<" combinatorially duplicate faces")<<endl;
}else
{
assert(tempF.rows() == F.rows());
cout<<GREENGIN("selfintersect created no duplicate faces")<<endl;
}
F = tempF;
}
#endif
}
// Prepare left-hand side
switch(nlhs)
{
case 5:
{
// Treat indices as reals
plhs[4] = mxCreateDoubleMatrix(IM.rows(),IM.cols(), mxREAL);
double * IMp = mxGetPr(plhs[4]);
VectorXd IMd = (IM.cast<double>().array()+1).matrix();
copy(&IMd.data()[0],&IMd.data()[0]+IMd.size(),IMp);
// Fallthrough
}
case 4:
{
// Treat indices as reals
plhs[3] = mxCreateDoubleMatrix(J.rows(),J.cols(), mxREAL);
double * Jp = mxGetPr(plhs[3]);
VectorXd Jd = (J.cast<double>().array()+1).matrix();
copy(&Jd.data()[0],&Jd.data()[0]+Jd.size(),Jp);
// Fallthrough
}
case 3:
{
// Treat indices as reals
plhs[2] = mxCreateDoubleMatrix(IF.rows(),IF.cols(), mxREAL);
double * IFp = mxGetPr(plhs[2]);
MatrixXd IFd = (IF.cast<double>().array()+1).matrix();
copy(&IFd.data()[0],&IFd.data()[0]+IFd.size(),IFp);
// Fallthrough
}
case 2:
{
// Treat indices as reals
plhs[1] = mxCreateDoubleMatrix(F.rows(),F.cols(), mxREAL);
double * Fp = mxGetPr(plhs[1]);
MatrixXd Fd = (F.cast<double>().array()+1).matrix();
copy(&Fd.data()[0],&Fd.data()[0]+Fd.size(),Fp);
// Fallthrough
}
case 1:
{
plhs[0] = mxCreateDoubleMatrix(V.rows(),V.cols(), mxREAL);
double * Vp = mxGetPr(plhs[0]);
copy(&V.data()[0],&V.data()[0]+V.size(),Vp);
break;
}
default:break;
}
// Restore the std stream buffer Important!
std::cout.rdbuf(outbuf);
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
if (nrhs<1) mexErrMsgTxt("usage: ptr = supportDetectmex(0,robot_obj,...); alpha=supportDetectmex(ptr,...,...)");
if (nlhs<1) mexErrMsgTxt("take at least one output... please.");
struct SupportDetectData* pdata;
// mxArray* pm;
double* pr;
int i,j;
if (mxGetScalar(prhs[0])==0) { // then construct the data object and return
pdata = new struct SupportDetectData;
// get robot mex model ptr
if (!mxIsNumeric(prhs[1]) || mxGetNumberOfElements(prhs[1])!=1)
mexErrMsgIdAndTxt("DRC:supportDetectmex:BadInputs","the second argument should be the robot mex ptr");
memcpy(&(pdata->r),mxGetData(prhs[1]),sizeof(pdata->r));
// get the map ptr back from matlab
if (!mxIsNumeric(prhs[2]) || mxGetNumberOfElements(prhs[2])!=1)
mexErrMsgIdAndTxt("DRC:supportDetectmex:BadInputs","the third argument should be the map ptr");
memcpy(&(pdata->map_ptr),mxGetPr(prhs[2]),sizeof(pdata->map_ptr));
if (!pdata->map_ptr)
mexWarnMsgTxt("Map ptr is NULL. Assuming flat terrain at z=0");
mxClassID cid;
if (sizeof(pdata)==4) cid = mxUINT32_CLASS;
else if (sizeof(pdata)==8) cid = mxUINT64_CLASS;
else mexErrMsgIdAndTxt("Drake:supportDetectmex:PointerSize","Are you on a 32-bit machine or 64-bit machine??");
plhs[0] = mxCreateNumericMatrix(1,1,cid,mxREAL);
memcpy(mxGetData(plhs[0]),&pdata,sizeof(pdata));
return;
}
// first get the ptr back from matlab
if (!mxIsNumeric(prhs[0]) || mxGetNumberOfElements(prhs[0])!=1)
mexErrMsgIdAndTxt("DRC:supportDetectmex:BadInputs","the first argument should be the ptr");
memcpy(&pdata,mxGetData(prhs[0]),sizeof(pdata));
int nq = pdata->r->num_positions;
int nv = pdata->r->num_velocities;
int narg=1;
double *q = mxGetPr(prhs[narg++]);
double *qd = &q[nq];
Map<VectorXd> qvec(q,nq);
Map<VectorXd> qdvec(qd, nv);
int desired_support_argid = narg++;
double* double_contact_sensor = mxGetPr(prhs[narg]); int len = static_cast<int>(mxGetNumberOfElements(prhs[narg++]));
VectorXi contact_sensor(len);
for (i=0; i<len; i++)
contact_sensor(i)=(int)double_contact_sensor[i];
double contact_threshold = mxGetScalar(prhs[narg++]);
double terrain_height = mxGetScalar(prhs[narg++]); // nonzero if we're using DRCFlatTerrainMap
int contact_logic_AND = (int) mxGetScalar(prhs[narg++]); // true if we should AND plan and sensor, false if we should OR them
KinematicsCache<double> cache = pdata->r->doKinematics(qvec, qdvec); // FIXME: pass this into the function.
//---------------------------------------------------------------------
// Compute active support from desired supports -----------------------
vector<SupportStateElement,Eigen::aligned_allocator<SupportStateElement>> active_supports;
set<int> contact_bodies; // redundant, clean up later
int num_active_contact_pts=0;
if (!mxIsEmpty(prhs[desired_support_argid])) {
VectorXd phi;
mxArray* mxBodies;
mxBodies = mxGetProperty(prhs[desired_support_argid],0,"bodies");
if (!mxBodies) mxBodies = mxGetField(prhs[desired_support_argid],0,"bodies");
if (!mxBodies) mexErrMsgTxt("couldn't get bodies");
double* pBodies = mxGetPr(mxBodies);
mxArray* mxContactPts;
// We may have gotten a RigidBodySupportState object (in which case we need mxGetProperty...
mxContactPts = mxGetProperty(prhs[desired_support_argid],0,"contact_pts");
// ...or a struct array, in which case we need mxGetField
if (!mxContactPts) mxContactPts = mxGetField(prhs[desired_support_argid],0,"contact_pts");
if (!mxContactPts) mexErrMsgTxt("couldn't get contact points");
for (i=0; i<mxGetNumberOfElements(mxBodies);i++) {
mxArray* mxBodyContactPts = mxGetCell(mxContactPts,i);
assert(mxGetM(mxBodyContactPts) == 3);
int nc = static_cast<int>(mxGetN(mxBodyContactPts));
if (nc<1) continue;
Map<MatrixXd> all_body_contact_pts(mxGetPr(mxBodyContactPts), mxGetM(mxBodyContactPts), mxGetN(mxBodyContactPts));
SupportStateElement se;
se.body_idx = (int) pBodies[i]-1;
for (j=0; j<nc; j++) {
se.contact_pts.push_back(all_body_contact_pts.col(j));
}
if (contact_threshold == -1) { // ignore terrain
if (contact_sensor(i)!=0) { // no sensor info, or sensor says yes contact
active_supports.push_back(se);
num_active_contact_pts += nc;
contact_bodies.insert((int)se.body_idx);
}
}
else {
contactPhi(pdata->r, cache, se, pdata->map_ptr, phi, terrain_height);
bool in_contact = true;
if (contact_logic_AND) { // plan is true, now check contact sensor/kinematics
in_contact = (phi.minCoeff()<=contact_threshold || contact_sensor(i)==1); // any contact below threshold (kinematically) OR contact sensor says yes contact
}
// else: (plan) OR (sensor), so in_contact = true
if (in_contact) {
active_supports.push_back(se);
num_active_contact_pts += nc;
contact_bodies.insert((int)se.body_idx);
}
}
}
}
if (nlhs>0) {
plhs[0] = mxCreateDoubleMatrix(1,static_cast<int>(active_supports.size()),mxREAL);
pr = mxGetPr(plhs[0]);
int i=0;
for (vector<SupportStateElement,Eigen::aligned_allocator<SupportStateElement>>::iterator iter = active_supports.begin(); iter!=active_supports.end(); iter++) {
pr[i++] = (double) (iter->body_idx + 1);
}
}
}
void mexFunction(int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[])
{
// This is useful for debugging whether Matlab is caching the mex binary
//mexPrintf("%s %s\n",__TIME__,__DATE__);
igl::matlab::MexStream mout;
std::streambuf *outbuf = std::cout.rdbuf(&mout);
#else
int main(int argc, char * argv[])
{
#endif
using namespace std;
using namespace Eigen;
using namespace igl;
using namespace igl::matlab;
using namespace igl::cgal;
MatrixXd V;
MatrixXi F;
igl::cgal::RemeshSelfIntersectionsParam params;
string prefix;
bool use_obj_format = false;
#ifdef MEX
if(nrhs < 2)
{
mexErrMsgTxt("nrhs < 2");
}
parse_rhs_double(prhs,V);
parse_rhs_index(prhs+1,F);
mexErrMsgTxt(V.cols()==3,"V must be #V by 3");
mexErrMsgTxt(F.cols()==3,"F must be #F by 3");
if(nrhs>2)
{
int i = 2;
while(i<nrhs)
{
if(!mxIsChar(prhs[i]))
{
mexErrMsgTxt("Parameter names should be char strings");
}
// Cast to char
const char * name = mxArrayToString(prhs[i]);
if(strcmp("DetectOnly",name) == 0)
{
validate_arg_scalar(i,nrhs,prhs,name);
validate_arg_logical(i,nrhs,prhs,name);
mxLogical * v = (mxLogical *)mxGetData(prhs[++i]);
params.detect_only = *v;
}else if(strcmp("FirstOnly",name) == 0)
{
validate_arg_scalar(i,nrhs,prhs,name);
validate_arg_logical(i,nrhs,prhs,name);
mxLogical * v = (mxLogical *)mxGetData(prhs[++i]);
params.first_only = *v;
}else
{
mexErrMsgTxt(C_STR("Unsupported parameter: "<<name));
}
i++;
}
}
#else
if(argc <= 1)
{
cerr<<"Usage:"<<endl<<" selfintersect [path to .off/.obj mesh] "
"[0 or 1 for detect only]"<<endl;
return 1;
}
// Apparently CGAL doesn't have a good data structure triangle soup. Their
// own examples use (V,F):
// http://www.cgal.org/Manual/latest/doc_html/cgal_manual/AABB_tree/Chapter_main.html#Subsection_64.3.7
//
// Load mesh
string filename(argv[1]);
if(!read_triangle_mesh(filename,V,F))
{
//cout<<REDRUM("Reading "<<filename<<" failed.")<<endl;
return false;
}
cout<<GREENGIN("Read "<<filename<<" successfully.")<<endl;
{
// dirname, basename, extension and filename
string dirname,b,ext;
pathinfo(filename,dirname,b,ext,prefix);
prefix = dirname + "/" + prefix;
transform(ext.begin(), ext.end(), ext.begin(), ::tolower);
use_obj_format = ext == "obj";
}
if(argc>2)
{
//http://stackoverflow.com/a/9748431/148668
char *p;
long d = strtol(argv[2], &p, 10);
if (errno != 0 || *p != '\0')
{
cerr<<"detect only param should be 0 or 1"<<endl;
}else
{
params.detect_only = d!=0;
}
}
#endif
MatrixXi IF;
VectorXi J,IM;
if(F.rows()>0)
{
// Check that there aren't any combinatorially or geometrically degenerate triangles
VectorXd A;
doublearea(V,F,A);
if(A.minCoeff()<=0)
{
#ifdef MEX
mexErrMsgTxt("Geometrically degenerate face found.");
#else
cerr<<"Geometrically degenerate face found."<<endl;
return 1;
#endif
}
VectorXi F12,F23,F31;
F12 = F.col(0)-F.col(1);
F23 = F.col(1)-F.col(2);
F31 = F.col(2)-F.col(0);
if(
F12.minCoeff() == 0 ||
F23.minCoeff() == 0 ||
F31.minCoeff() == 0)
{
#ifdef MEX
mexErrMsgTxt("Combinatorially degenerate face found.");
#else
cerr<<"Geometrically degenerate face found."<<endl;
return 1;
#endif
}
// Now mesh self intersections
{
MatrixXd tempV;
MatrixXi tempF;
remesh_self_intersections(V,F,params,tempV,tempF,IF,J,IM);
//cout<<BLUEGIN("Found and meshed "<<IF.rows()<<" pair"<<(IF.rows()==1?"":"s")
// <<" of self-intersecting triangles.")<<endl;
V=tempV;
F=tempF;
#ifndef MEX
cout<<"writing pair list to "<<(prefix+"-IF.dmat")<<endl;
writeDMAT((prefix+"-IF.dmat").c_str(),IF);
cout<<"writing map to F list to "<<(prefix+"-J.dmat")<<endl;
writeDMAT((prefix+"-J.dmat").c_str(),J);
cout<<"writing duplicat index map to "<<(prefix+"-IM.dmat")<<endl;
writeDMAT((prefix+"-IM.dmat").c_str(),IM);
if(!params.detect_only)
{
if(use_obj_format)
{
cout<<"writing mesh to "<<(prefix+"-selfintersect.obj")<<endl;
writeOBJ(prefix+"-selfintersect.obj",V,F);
}else
{
cout<<"writing mesh to "<<(prefix+"-selfintersect.off")<<endl;
writeOFF(prefix+"-selfintersect.off",V,F);
}
}
#endif
}
// Double-check output
#ifdef DEBUG
// There should be *no* combinatorial duplicates
{
MatrixXi tempF;
unique_simplices(F,tempF);
if(tempF.rows() < F.rows())
{
cout<<REDRUM("Error: selfintersect created "<<
F.rows()-tempF.rows()<<" combinatorially duplicate faces")<<endl;
}else
{
assert(tempF.rows() == F.rows());
cout<<GREENGIN("selfintersect created no duplicate faces")<<endl;
}
F = tempF;
}
#endif
}
#ifdef MEX
// Prepare left-hand side
switch(nlhs)
{
case 5:
{
// Treat indices as reals
plhs[4] = mxCreateDoubleMatrix(IM.rows(),IM.cols(), mxREAL);
double * IMp = mxGetPr(plhs[4]);
VectorXd IMd = (IM.cast<double>().array()+1).matrix();
copy(&IMd.data()[0],&IMd.data()[0]+IMd.size(),IMp);
// Fallthrough
}
case 4:
{
// Treat indices as reals
plhs[3] = mxCreateDoubleMatrix(J.rows(),J.cols(), mxREAL);
double * Jp = mxGetPr(plhs[3]);
VectorXd Jd = (J.cast<double>().array()+1).matrix();
copy(&Jd.data()[0],&Jd.data()[0]+Jd.size(),Jp);
// Fallthrough
}
case 3:
{
// Treat indices as reals
plhs[2] = mxCreateDoubleMatrix(IF.rows(),IF.cols(), mxREAL);
double * IFp = mxGetPr(plhs[2]);
MatrixXd IFd = (IF.cast<double>().array()+1).matrix();
copy(&IFd.data()[0],&IFd.data()[0]+IFd.size(),IFp);
// Fallthrough
}
case 2:
{
// Treat indices as reals
plhs[1] = mxCreateDoubleMatrix(F.rows(),F.cols(), mxREAL);
double * Fp = mxGetPr(plhs[1]);
MatrixXd Fd = (F.cast<double>().array()+1).matrix();
copy(&Fd.data()[0],&Fd.data()[0]+Fd.size(),Fp);
// Fallthrough
}
case 1:
{
plhs[0] = mxCreateDoubleMatrix(V.rows(),V.cols(), mxREAL);
double * Vp = mxGetPr(plhs[0]);
copy(&V.data()[0],&V.data()[0]+V.size(),Vp);
break;
}
default:break;
}
// Restore the std stream buffer Important!
std::cout.rdbuf(outbuf);
#else
return 0;
#endif
}
//#define IGL_LINPROG_VERBOSE
IGL_INLINE bool igl::linprog(
const Eigen::VectorXd & c,
const Eigen::MatrixXd & _A,
const Eigen::VectorXd & b,
const int k,
Eigen::VectorXd & x)
{
// This is a very literal translation of
// http://www.mathworks.com/matlabcentral/fileexchange/2166-introduction-to-linear-algebra/content/strang/linprog.m
using namespace Eigen;
using namespace std;
bool success = true;
// number of constraints
const int m = _A.rows();
// number of original variables
const int n = _A.cols();
// number of iterations
int it = 0;
// maximum number of iterations
//const int MAXIT = 10*m;
const int MAXIT = 100*m;
// residual tolerance
const double tol = 1e-10;
const auto & sign = [](const Eigen::VectorXd & B) -> Eigen::VectorXd
{
Eigen::VectorXd Bsign(B.size());
for(int i = 0;i<B.size();i++)
{
Bsign(i) = B(i)>0?1:(B(i)<0?-1:0);
}
return Bsign;
};
// initial (inverse) basis matrix
VectorXd Dv = sign(sign(b).array()+0.5);
Dv.head(k).setConstant(1.);
MatrixXd D = Dv.asDiagonal();
// Incorporate slack variables
MatrixXd A(_A.rows(),_A.cols()+D.cols());
A<<_A,D;
// Initial basis
VectorXi B = igl::colon<int>(n,n+m-1);
// non-basis, may turn out that vector<> would be better here
VectorXi N = igl::colon<int>(0,n-1);
int j;
double bmin = b.minCoeff(&j);
int phase;
VectorXd xb;
VectorXd s;
VectorXi J;
if(k>0 && bmin<0)
{
phase = 1;
xb = VectorXd::Ones(m);
// super cost
s.resize(n+m+1);
s<<VectorXd::Zero(n+k),VectorXd::Ones(m-k+1);
N.resize(n+1);
N<<igl::colon<int>(0,n-1),B(j);
J.resize(B.size()-1);
// [0 1 2 3 4]
// ^
// [0 1]
// [3 4]
J.head(j) = B.head(j);
J.tail(B.size()-j-1) = B.tail(B.size()-j-1);
B(j) = n+m;
MatrixXd AJ;
igl::slice(A,J,2,AJ);
const VectorXd a = b - AJ.rowwise().sum();
{
MatrixXd old_A = A;
A.resize(A.rows(),A.cols()+a.cols());
A<<old_A,a;
}
D.col(j) = -a/a(j);
D(j,j) = 1./a(j);
}else if(k==m)
{
phase = 2;
xb = b;
s.resize(c.size()+m);
// cost function
s<<c,VectorXd::Zero(m);
}else //k = 0 or bmin >=0
{
phase = 1;
xb = b.array().abs();
s.resize(n+m);
// super cost
s<<VectorXd::Zero(n+k),VectorXd::Ones(m-k);
}
while(phase<3)
{
double df = -1;
int t = std::numeric_limits<int>::max();
// Lagrange mutipliers fro Ax=b
VectorXd yb = D.transpose() * igl::slice(s,B);
while(true)
{
if(MAXIT>0 && it>=MAXIT)
{
#ifdef IGL_LINPROG_VERBOSE
cerr<<"linprog: warning! maximum iterations without convergence."<<endl;
#endif
success = false;
break;
}
// no freedom for minimization
if(N.size() == 0)
{
break;
}
// reduced costs
VectorXd sN = igl::slice(s,N);
MatrixXd AN = igl::slice(A,N,2);
VectorXd r = sN - AN.transpose() * yb;
int q;
// determine new basic variable
double rmin = r.minCoeff(&q);
// optimal! infinity norm
if(rmin>=-tol*(sN.array().abs().maxCoeff()+1))
{
break;
}
// increment iteration count
it++;
// apply Bland's rule to avoid cycling
if(df>=0)
{
if(MAXIT == -1)
{
#ifdef IGL_LINPROG_VERBOSE
cerr<<"linprog: warning! degenerate vertex"<<endl;
#endif
success = false;
}
igl::find((r.array()<0).eval(),J);
double Nq = igl::slice(N,J).minCoeff();
// again seems like q is assumed to be a scalar though matlab code
// could produce a vector for multiple matches
(N.array()==Nq).cast<int>().maxCoeff(&q);
}
VectorXd d = D*A.col(N(q));
VectorXi I;
igl::find((d.array()>tol).eval(),I);
if(I.size() == 0)
{
#ifdef IGL_LINPROG_VERBOSE
cerr<<"linprog: warning! solution is unbounded"<<endl;
#endif
// This seems dubious:
it=-it;
success = false;
break;
}
VectorXd xbd = igl::slice(xb,I).array()/igl::slice(d,I).array();
// new use of r
int p;
{
double r;
r = xbd.minCoeff(&p);
p = I(p);
// apply Bland's rule to avoid cycling
if(df>=0)
{
igl::find((xbd.array()==r).eval(),J);
double Bp = igl::slice(B,igl::slice(I,J)).minCoeff();
// idiotic way of finding index in B of Bp
// code down the line seems to assume p is a scalar though the matlab
// code could find a vector of matches)
(B.array()==Bp).cast<int>().maxCoeff(&p);
}
// update x
xb -= r*d;
xb(p) = r;
// change in f
df = r*rmin;
}
// row vector
RowVectorXd v = D.row(p)/d(p);
yb += v.transpose() * (s(N(q)) - d.transpose()*igl::slice(s,B));
d(p)-=1;
// update inverse basis matrix
D = D - d*v;
t = B(p);
B(p) = N(q);
if(t>(n+k-1))
{
// remove qth entry from N
VectorXi old_N = N;
N.resize(N.size()-1);
N.head(q) = old_N.head(q);
N.head(q) = old_N.head(q);
N.tail(old_N.size()-q-1) = old_N.tail(old_N.size()-q-1);
}else
{
N(q) = t;
}
}
// iterative refinement
xb = (xb+D*(b-igl::slice(A,B,2)*xb)).eval();
// must be due to rounding
VectorXi I;
igl::find((xb.array()<0).eval(),I);
if(I.size()>0)
{
// so correct
VectorXd Z = VectorXd::Zero(I.size(),1);
igl::slice_into(Z,I,xb);
}
// B, xb,n,m,res=A(:,B)*xb-b
if(phase == 2 || it<0)
{
break;
}
if(xb.transpose()*igl::slice(s,B) > tol)
{
it = -it;
#ifdef IGL_LINPROG_VERBOSE
cerr<<"linprog: warning, no feasible solution"<<endl;
#endif
success = false;
break;
}
// re-initialize for Phase 2
phase = phase+1;
s*=1e6*c.array().abs().maxCoeff();
s.head(n) = c;
}
x.resize(std::max(B.maxCoeff()+1,n));
igl::slice_into(xb,B,x);
x = x.head(n).eval();
return success;
}
bool ClassLayouter::computeNormally()
{
FuzzyDependAttr::Ptr fuzzyAttr = m_parent->getAttr<FuzzyDependAttr>();
if (!fuzzyAttr)
return false;
Graph G;
GraphAttributes GA(G,
GraphAttributes::nodeGraphics | GraphAttributes::edgeGraphics |
GraphAttributes::nodeLabel | GraphAttributes::nodeTemplate |
GraphAttributes::edgeDoubleWeight);
SparseMatrix& veMat = fuzzyAttr->vtxEdgeMatrix();
VectorXd edgeWeight = fuzzyAttr->edgeWeightVector();
if (edgeWeight.size() != veMat.cols())
{
m_status |= WARNING_USE_DEFAULT_EDGE_WEIGHT;
edgeWeight.setOnes(veMat.cols());
}
const int nNodes = veMat.rows();
const int nEdges = veMat.cols();
if (nNodes <= 0 || nNodes != m_childList.size() || nEdges < 1)
{
m_status |= WARNING_NO_EDGE;
return false;
}
bool isUseOrthoLayout = nEdges < 50;
vector<node> nodeArray;
vector<edge> edgeArray;
NodeArray<float> nodeSize(G);
EdgeArray<double> edgeLength(G);
for (int i = 0; i < nNodes; ++i)
{
nodeArray.push_back(G.newNode());
float r = m_nodeRadius[i];
GA.width(nodeArray.back()) = r*2;
GA.height(nodeArray.back()) = r*2;
nodeSize[nodeArray.back()] = r * 2;
}
float maxEdgeWeight = edgeWeight.maxCoeff();
float minEdgeWeight = edgeWeight.minCoeff();
for (int ithEdge = 0; ithEdge < nEdges; ++ithEdge)
{
int src, dst;
GraphUtility::getVtxFromEdge(veMat, ithEdge, src, dst);
edgeArray.push_back(G.newEdge(nodeArray[src], nodeArray[dst]));
//GA.doubleWeight(edgeArray.back()) = edgeWeight[ithEdge];
edgeLength[edgeArray.back()] = 1;//log(edgeWeight[ithEdge] + 1);
}
// add dummy vertices and edges in order to merge parallel edge segments attached to the same node
VectorXi compVec;
int nComp = 1;
const float dummyNodeRadius = 0.01;
if(m_isMergeEdgeEnd && isUseOrthoLayout && GraphUtility::findConnectedComponentsVE( veMat, compVec, nComp ))
{
bool* isCompSet = new bool[nComp];
for (int i = 0; i < nComp; ++i)
isCompSet[i] = false;
// add a dummy node and edge for each connect component
for (int ithVtx = 0; ithVtx < compVec.size(); ++ithVtx)
{
int ithCmp = compVec[ithVtx];
if (isCompSet[ithCmp] == false)
{
// add dummy node and set its radius
nodeArray.push_back(G.newNode());
GA.width(nodeArray.back()) = dummyNodeRadius;
GA.height(nodeArray.back()) = dummyNodeRadius;
// add dummy edge
edgeArray.push_back(G.newEdge(nodeArray[ithVtx], nodeArray.back()));
isCompSet[ithCmp] = true;
}
}
delete[] isCompSet;
}
MatrixXd pos;
pos.resize(nNodes, 2);
try
{
if (isUseOrthoLayout)
{
PlanarizationLayout layouter;
OrthoLayout *ol = new OrthoLayout;
float sep = max(m_nodeRadius.sum() / nNodes, LayoutSetting::s_baseRadius * 12);
ol->separation(sep);
ol->cOverhang(0.1);
ol->setOptions(1+4);
layouter.setPlanarLayouter(ol);
layouter.call(GA);
for (int v = 0; v < nNodes; ++v)
{
double x = GA.x(nodeArray[v]);
double y = GA.y(nodeArray[v]);
pos(v,0) = x;
pos(v,1) = y;
}
}
else
{
FMMMLayout layouter;
//CircularLayout layouter;
float avgRadius = m_nodeRadius.sum();
layouter.unitEdgeLength(avgRadius * 4);
//layouter.call(GA, edgeLength);
layouter.call(GA);
// layouter.resizeDrawing(true);
// layouter.resizingScalar(3);
for (int v = 0; v < nNodes; ++v)
{
double x = GA.x(nodeArray[v]);
double y = GA.y(nodeArray[v]);
pos(v,0) = x;
pos(v,1) = y;
}
MDSPostProcesser m_postProcessor(5000, 1, 1.0, 1.0, LayoutSetting::s_baseRadius * 2);
m_postProcessor.set2DPos(pos, m_nodeRadius.cast<double>());
m_postProcessor.compute();
m_postProcessor.getFinalPos(pos);
}
}
catch(...)//AlgorithmFailureException e
{
return false;
}
VectorXd halfSize,offset;
GeometryUtility::moveToOrigin(pos, m_nodeRadius.cast<double>(), halfSize, &offset);
m_nodePos = pos.cast<float>();
// postprocessing, and set edges
float edgeBound[2] = {halfSize[0], halfSize[1]};
if (isUseOrthoLayout)
{
for(int ithEdge = 0; ithEdge < nEdges; ++ithEdge)
{
DPolyline& p = GA.bends(edgeArray[ithEdge]);
int nPoints = p.size();
BSpline splineBuilder(5, BSpline::BSPLINE_OPEN_UNIFORM, true);
int ithPnt = 0;
for (DPolyline::iterator pLine = p.begin(); pLine != p.end(); ++pLine, ++ithPnt)
{
float px = (*pLine).m_x + offset[0];
float py = (*pLine).m_y + offset[1];
splineBuilder.addPoint(px,py);
splineBuilder.addPoint(px,py);
}
splineBuilder.computeLine((nPoints) * 5);
const QList<QPointF>& curve = splineBuilder.getCurvePnts();
m_edgeParam.push_back(EdgeParam());
EdgeParam& ep = m_edgeParam.back();
ep.m_points.resize(curve.size(),2);
for (int i = 0; i < curve.size(); ++i)
{
ep.m_points(i, 0) = curve[i].x();
ep.m_points(i, 1) = curve[i].y();
edgeBound[0] = qMax(edgeBound[0], (float)qAbs(curve[i].x()));
edgeBound[1] = qMax(edgeBound[1], (float)qAbs(curve[i].y()));
}
ep.m_weight = edgeWeight[ithEdge];
}
}
else
{
DelaunayCore::DelaunayRouter router;
router.setLaneWidthRatio(2);
router.setSmoothParam(0.5,2);
router.setEndPointNormalRatio(0.8);
computeEdgeRoute(router);
}
m_totalRadius = qSqrt(edgeBound[0]*edgeBound[0] + edgeBound[1]*edgeBound[1]);
return true;
}
void
AutoSeg::autoSegment()
{
// Total timing
double last = getTime ();
// Step 1: filter the input cloud
//double last_f = pcl::getTime ();
SampleFilter filter (input_cloud_);
filter.setMaxPtLimit (input_task_->getMaxPatchDistance());
filter.filter();
filtered_cloud_ = filter.cloud_filtered_;
//double now_f = pcl::getTime ();
//cerr << "Time for filter: " << now_f-last_f << endl;
// Return if no points are valid
if (!AutoSeg::numValidPoints(filtered_cloud_))
return;
// Update the viewer
if (do_vis_ && show_filtered_ && viewer_)
AutoSeg::showFilteredCloud();
// Decimation ratio
float dec_ratio = static_cast<float> (nd) /
(2.0*static_cast<float> (filtered_cloud_->points.size ()));
// Step 2: find salient points on the filtered cloud
//double last_sal = pcl::getTime ();
sal_ = new SampleSaliency;
sal_->setInputCloud (filtered_cloud_);
sal_->setNNSize (2.0f*radius_, 580.0f/dec_ratio);
sal_->setNNCAThres (MAX_DON_COS_ANGLE);
if (use_gravity_) {sal_->setG (g_);}
if (use_gravity_) {sal_->setNGCAThres (MAX_DONG_COS_ANGLE);}
sal_->setNMS(do_nms_);
sal_->extractSalientPoints ();
// Keep only MAX_SAL_POINTS random points
// TBD: do it in sal_
if ((sal_->getIndxSalient())->size() > MAX_SAL_POINTS)
{
random_shuffle((sal_->getIndxSalient())->begin(), (sal_->getIndxSalient())->end());
(sal_->getIndxSalient())->resize (MAX_SAL_POINTS);
}
//double now_sal = pcl::getTime ();
//cerr << "Time for sal: " << now_sal-last_sal << endl;
// Update viewer with normals
if (do_vis_ && viewer_)
{
sal_->setViewer (viewer_);
if (show_sal_filtered_) sal_->showDtFPCloud ();
if (show_fixation_) sal_->showFixation ();
if (show_salient_) sal_->showSalPoints (true);
if (show_sal_curvatures_) sal_->showCurvature (true);
if (show_sal_normals_) sal_->showNormals (true);
}
if (do_vis_ && show_patches_ && viewer_) removePatchPlots();
// Create nn object
search::OrganizedNeighbor<PointXYZ> search;
search.setInputCloud(filtered_cloud_);
// For each seed point
ns = 0; // set number of current seeds to zero
int num_patches = 0;
for (int si=0; si<(sal_->getIndxSalient())->size (); si++)
{
// generate seeds until some termination condition holds
//if (AutoSeg::terminate())
// break;
// Step 3: generate and validate a new seed
seed = (sal_->getIndxSalient())->at(si);
if (!isSeedValid())
continue;
seeds.push_back(seed);
ns++;
// Step 4: fetch and validate neighborhood
//double last_nn = pcl::getTime ();
search.radiusSearch(filtered_cloud_->points[seed], radius_, nn_indices, nn_sqr_distances);
nn_cloud_->points.resize (0);
for (int i=0; i<nn_indices.size(); i++)
nn_cloud_->points.push_back(filtered_cloud_->points[nn_indices[i]]);
//double now_nn = pcl::getTime ();
//cerr << "Time for nn: " << now_nn-last_nn << endl;
if (do_vis_ && show_nn_ && viewer_)
AutoSeg::showNN();
// Step 5: fit the patch
//double last_pf = pcl::getTime ();
PatchFit *patchFit;
patchFit = new PatchFit(nn_cloud_);
patchFit->setSSmax (50);
patchFit->fit();
//double now_pf = pcl::getTime ();
//cerr << "Time for fitting " << nn_cloud_->points.size() <<
// " points: " << now_pf-last_pf << endl;
// Step 6: validate patch
//double last_val = pcl::getTime ();
if (do_validation_)
{
(*patchFit->p_).computeResidual (nn_cloud_);
if ((*patchFit->p_).getResidual() > t_residual_)
continue;
VectorXd k;
k = (*patchFit->p_).getK ();
if (k.minCoeff () < t_min_curv_ || k.maxCoeff ()>t_max_curv_)
continue;
}
num_patches++;
//double now_val = pcl::getTime ();
//cerr << "Time for validation: " << now_val-last_val << endl;
// Visualize patch
//double last_pp = pcl::getTime ();
if (do_vis_ && show_patches_ && viewer_)
{
PatchPlot *patch_plot;
(*patchFit->p_).setID (ns);
(*patchFit->p_).setSS (0.025);
(*patchFit->p_).infer_params();
(*patchFit->p_).gs();
//PatchPlot patch_plot (*patchFit->p_);
patch_plot = new PatchPlot (*patchFit->p_);
patch_plot->showPatch (viewer_, 0, 1, 0, boost::to_string(ns) + "_patch");
delete (patch_plot);
}
//double now_pp = pcl::getTime ();
//cerr << "Time for ploting patch: " << now_pp-last_pp << endl;
if (!no_stats_)
{
// Save normal vector
(*patchFit->p_).setCnn (sal_->getNormalNormalAngle (seed));
if (use_gravity_)
(*patchFit->p_).setCng (sal_->getNormalGravityAngle (seed));
// Step 7: save statistics
AutoSeg::saveStat (*patchFit->p_);
// Step 8: print statistics
Vector2d sk = (*patchFit->p_).getSK();
cout << "patch stats" << " "
<< sk (0) << " "
<< sk (1) << " "
<< (*patchFit->p_).getCnn () << " "
<< (*patchFit->p_).getCng () << endl;
}
// delete objects
delete (patchFit);
} // for each seed
if(do_vis_ && show_patches_) max_patch_plot_id_ = ns-1;
else max_patch_plot_id_ = 0;
// Total timing
double now = getTime();
cerr << "Total autoseg for " << num_patches << " patches out of " << ns <<
" seed points: " << now-last << " sec." << endl;
// destroy objects
delete (sal_);
}
double DataGenerator::evaluate(MatrixXd estimate) {
if (estimate.rows()!=mCenters.rows()) {
cout << endl <<"ERROR:DataGenerator evaluate"<< endl;
cout << "resultEvaluation dimension mismatch "<< endl;
return 0;
}
//MatrixXd finalEstimate(mPara.dimension(), mPara.cluster());
unsigned long nEstimate = estimate.cols();
//cout << "finalEstimate row:"<< finalEstimate.rows() << endl;
//cout << "estimate row:"<< estimate.rows() << endl;
//cout << "nEstimate:"<< nEstimate <<" mPara.cluster():"<< mPara.cluster() << endl;
//finalEstimate.leftCols(nEstimate) = estimate;
MatrixXd finalEstimate = estimate;
/*
if (nEstimate != mPara.cluster()) {
unsigned long sizeDiff = mPara.cluster() - nEstimate;
if (sizeDiff>0) {
finalEstimate.rightCols(sizeDiff) = MatrixXd::Zero(mPara.dimension(), sizeDiff);
cout << "# of decomposed element is less than expected!! expect:" << mPara.cluster() << " only:" << nEstimate << endl;
nEstimate = nEstimate + sizeDiff;
} else {
cout << endl << "ERROR:DataGenerator evaluate" << endl;
cout <<"Decompsed center is more than expected. We already pick dominant centers!!" << endl;
}
}
*/
// Assign all the real center to the nearest estimater center
MatrixXd bestMatch(mPara.dimension(), mPara.cluster());
double error = 0;
for (unsigned long i=0; i< mPara.cluster(); i++) {
MatrixXd currentRep = mCenters.col(i).replicate(1,nEstimate);
MatrixXd diff = finalEstimate - currentRep;
diff = diff.array().pow(2);
VectorXd dist = diff.colwise().sum();
MatrixXf::Index minRow, minCol;
error += pow(dist.minCoeff(&minRow, &minCol),0.5);
bestMatch.col(i) = finalEstimate.col(minRow);
}
//cout << endl <<" ===== Best match ===== " << endl;
//cout << bestMatch << endl;
//cout << " ===== Original ===== " << endl;
//cout << mCenters << endl;
// cout << endl << "Avg RMSE=" << error/ mPara.cluster() << endl;
return (error/ mPara.cluster());
}