GainPair continuous_gini_gain(const VectorXd &values, const VectorXi &classes) {
IntsSet unique_classes(classes.data(),classes.data() + classes.size());
if(unique_classes.size() == 1) return std::make_pair(0, 0);
VectorXi lower_contingency_table = VectorXi::Zero(unique_classes.size());
// upper_contingency_table are the counts for each class
VectorXi upper_contingency_table = get_cross_table(classes);
double total_gini = gini(upper_contingency_table.cast<double>());
double best_gain = 0;
double best_threshold = 0;
Ints indices = argsort<double>(values.data(), values.size());
IntsSet::iterator begin = unique_classes.begin();
unsigned int n_classes = classes.size();
// Scan the classes in the order obtained from sorting the values
for (unsigned int i = 0; i < n_classes; ++i) {
int current_class = classes[indices[i]]; //
IntsSet::iterator it = unique_classes.find(current_class);
unsigned int position = std::distance(begin, it);
lower_contingency_table(position) += 1;
upper_contingency_table(position) -= 1;
double P_low = 1. * i / n_classes;
double G_low = gini(lower_contingency_table.cast<double>());
double P_upp = 1. * (n_classes - i) / n_classes;
double G_upp = gini(upper_contingency_table.cast<double>());
double G = P_low * G_low + P_upp * G_upp;
double g = total_gini - G;
if(g > best_gain) {
best_gain = g;
best_threshold = values[indices[i]];
}
}
return std::make_pair(best_gain, best_threshold);
}
void AddSingleTimeLinearPostureConstraint(
const double *t, const RigidBodyConstraint* constraint, int nq,
const drake::solvers::DecisionVariableView& vars,
OptimizationProblem* prog) {
DRAKE_ASSERT(
constraint->getCategory() ==
RigidBodyConstraint::SingleTimeLinearPostureConstraintCategory);
const SingleTimeLinearPostureConstraint* st_lpc =
static_cast<const SingleTimeLinearPostureConstraint*>(constraint);
if (!st_lpc->isTimeValid(t)) { return; }
VectorXd lb;
VectorXd ub;
st_lpc->bounds(t, lb, ub);
VectorXi iAfun;
VectorXi jAvar;
VectorXd A;
st_lpc->geval(t, iAfun, jAvar, A);
DRAKE_ASSERT(iAfun.size() == jAvar.size());
DRAKE_ASSERT(iAfun.size() == A.size());
typedef Eigen::Triplet<double> T;
std::vector<T> triplet_list;
for (int i = 0; i < iAfun.size(); i++) {
triplet_list.push_back(T(iAfun[i], jAvar[i], A[i]));
}
Eigen::SparseMatrix<double> A_sparse(
st_lpc->getNumConstraint(t), nq);
A_sparse.setFromTriplets(triplet_list.begin(), triplet_list.end());
prog->AddLinearConstraint(MatrixXd(A_sparse), lb, ub, {vars});
}
bool IOUSet::cmpIOU(const VectorXi &a, const VectorXi &b, float max_iou) const {
assert( a.size() == b.size() );
const int N = a.size();
const float t = max_iou / (1 + max_iou);
const float t_inter = t*(a[N-1]+b[N-1]);
float upper = std::min(a[N-1], b[N-1]), lower = std::max(a[N-1]+b[N-1]-cnt_[N-1],0);
if( lower >= t_inter ) return true;
if( upper <= t_inter ) return false;
std::queue<int> q;
q.push( N-1 );
while(!q.empty()) {
int n = q.front();
q.pop();
// Split the current node
float c_lower = std::max(a[n]+b[n]-cnt_[n],0), c_upper = std::min(a[n],b[n]);
float left_lower = std::max(a[left_[n]]+b[left_[n]]-cnt_[left_[n]],0), left_upper = std::min(a[left_[n]],b[left_[n]]);
float right_lower = std::max(a[right_[n]]+b[right_[n]]-cnt_[right_[n]],0), right_upper = std::min(a[right_[n]],b[right_[n]]);
lower += left_lower+right_lower-c_lower;
upper += left_upper+right_upper-c_upper;
if( lower >= t_inter ) return true;
if( upper <= t_inter ) return false;
if( left_lower < left_upper && left_[left_[n]] != -1 )
q.push( left_[n] );
if( right_lower < right_upper && right_[right_[n]] != -1 )
q.push( right_[n] );
}
printf("l:%f u:%f\n", lower, upper );
return true;
}
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray *prhs[])
{
if(nrhs!=3 || nlhs != 8)
{
mexErrMsgIdAndTxt("Drake:testMultipleTimeLinearPostureConstrainttmex:BadInputs","Usage [num_cnst,cnst_val,iAfun,jAvar,A,cnst_name,lb,ub] = testMultipleTimeLinearPostureConstraintmex(kinCnst,q,t)");
}
MultipleTimeLinearPostureConstraint* cnst = (MultipleTimeLinearPostureConstraint*) getDrakeMexPointer(prhs[0]);
int n_breaks = mxGetNumberOfElements(prhs[2]);
double* t_ptr = new double[n_breaks];
memcpy(t_ptr,mxGetPrSafe(prhs[2]),sizeof(double)*n_breaks);
int nq = cnst->getRobotPointer()->num_positions;
MatrixXd q(nq,n_breaks);
if(mxGetM(prhs[1]) != nq || mxGetN(prhs[1]) != n_breaks)
{
mexErrMsgIdAndTxt("Drake:testMultipleTimeLinearPostureConstraintmex:BadInputs","Argument 2 must be of size nq*n_breaks");
}
memcpy(q.data(),mxGetPrSafe(prhs[1]),sizeof(double)*nq*n_breaks);
int num_cnst = cnst->getNumConstraint(t_ptr,n_breaks);
VectorXd c(num_cnst);
cnst->feval(t_ptr,n_breaks,q,c);
VectorXi iAfun;
VectorXi jAvar;
VectorXd A;
cnst->geval(t_ptr,n_breaks,iAfun,jAvar,A);
std::vector<std::string> cnst_names;
cnst->name(t_ptr,n_breaks,cnst_names);
VectorXd lb(num_cnst);
VectorXd ub(num_cnst);
cnst->bounds(t_ptr,n_breaks,lb,ub);
VectorXd iAfun_tmp(iAfun.size());
VectorXd jAvar_tmp(jAvar.size());
for(int i = 0;i<iAfun.size();i++)
{
iAfun_tmp(i) = (double) iAfun(i)+1;
jAvar_tmp(i) = (double) jAvar(i)+1;
}
plhs[0] = mxCreateDoubleScalar((double) num_cnst);
plhs[1] = mxCreateDoubleMatrix(num_cnst,1,mxREAL);
memcpy(mxGetPrSafe(plhs[1]),c.data(),sizeof(double)*num_cnst);
plhs[2] = mxCreateDoubleMatrix(iAfun_tmp.size(),1,mxREAL);
memcpy(mxGetPrSafe(plhs[2]),iAfun_tmp.data(),sizeof(double)*iAfun_tmp.size());
plhs[3] = mxCreateDoubleMatrix(jAvar_tmp.size(),1,mxREAL);
memcpy(mxGetPrSafe(plhs[3]),jAvar_tmp.data(),sizeof(double)*jAvar_tmp.size());
plhs[4] = mxCreateDoubleMatrix(A.size(),1,mxREAL);
memcpy(mxGetPrSafe(plhs[4]),A.data(),sizeof(double)*A.size());
int name_ndim = 1;
mwSize name_dims[] = {(mwSize) num_cnst};
plhs[5] = mxCreateCellArray(name_ndim,name_dims);
mxArray *name_ptr;
for(int i = 0;i<num_cnst;i++)
{
name_ptr = mxCreateString(cnst_names[i].c_str());
mxSetCell(plhs[5],i,name_ptr);
}
plhs[6] = mxCreateDoubleMatrix(num_cnst,1,mxREAL);
plhs[7] = mxCreateDoubleMatrix(num_cnst,1,mxREAL);
memcpy(mxGetPrSafe(plhs[6]),lb.data(),sizeof(double)*num_cnst);
memcpy(mxGetPrSafe(plhs[7]),ub.data(),sizeof(double)*num_cnst);
delete[] t_ptr;
}
void initMtlOptDM(pMtlOptDM dm){
dm->setTotalFrames(T);
dm->setDamping(ak,am);
dm->fixHeadFrames(fixHead);
dm->fixTailFrames(fixTail);
dm->Z = MatrixXd::Random(rs,T);
dm->Z.leftCols(fixHead).setZero();
dm->Z.rightCols(fixTail).setZero();
dm->initVariables(lambda,rs);
dm->S = MatrixXd::Random(rw,rs);
dm->conFrames.clear();
dm->conNodes.clear();
dm->uc.clear();
VectorXi conF;
conF.resize(2);
for (int f = 0; f < conF.size(); ++f){
conF[f] = fixHead+f;
}
for (int i = 0; i < conF.size(); ++i){
dm->conFrames.push_back(conF[i]);
vector<int> nodes;
for (int j = 0; j < 2; ++j){
nodes.push_back(i+j);
}
dm->conNodes.push_back(nodes);
dm->uc.push_back(VectorXd::Random(nodes.size()*3)*0.0f);
}
}
inline Matrix<T, Dynamic, Dynamic> pick_row(const Matrix<T, Dynamic, Dynamic>& M, const VectorXi& ind)
{
Matrix<T, Dynamic, Dynamic> N(ind.size(), M.cols());
for(unsigned i = 0; i < ind.size(); ++i)
N.row(i) = M.row(ind(i));
return N;
}
FiffEvoked MNEEpochDataList::average(FiffInfo& info, fiff_int_t first, fiff_int_t last, VectorXi sel, bool proj)
{
FiffEvoked p_evoked;
printf("Calculate evoked... ");
MatrixXd matAverage;
if(this->size() > 0)
matAverage = MatrixXd::Zero(this->at(0)->epoch.rows(), this->at(0)->epoch.cols());
else
{
p_evoked.aspect_kind = FIFFV_ASPECT_STD_ERR;
return p_evoked;
}
if(sel.size() > 0)
{
p_evoked.nave = sel.size();
for(qint32 i = 0; i < sel.size(); ++i)
matAverage.array() += this->at(sel(i))->epoch.array();
}
else
{
p_evoked.nave = this->size();
for(qint32 i = 0; i < this->size(); ++i)
matAverage.array() += this->at(i)->epoch.array();
}
matAverage.array() /= p_evoked.nave;
printf("%d averages used [done]\n ", p_evoked.nave);
p_evoked.setInfo(info, proj);
p_evoked.aspect_kind = FIFFV_ASPECT_AVERAGE;
p_evoked.first = first;
p_evoked.last = last;
RowVectorXf times = RowVectorXf(last-first+1);
for (qint32 k = 0; k < times.size(); ++k)
times[k] = ((float)(first+k)) / info.sfreq;
p_evoked.times = times;
p_evoked.comment = QString::number(this->at(0)->event);
if(p_evoked.proj.rows() > 0)
{
matAverage = p_evoked.proj * matAverage;
printf("\tSSP projectors applied to the evoked data\n");
}
p_evoked.data = matAverage;
return p_evoked;
}
/* --- DEPRECATED --- */
/* DPC */void ActiveSet::
/* DPC */permuteRows( const VectorXi & P )
/* DPC */{
/* DPC */ assert( false&&"DEPRECATED" );
/* DPC */ assert(P.size()==int(nba));
/* DPC */ VectorXi Pt(P.size());
/* DPC */ for( unsigned int i=0;i<nba;++i ) Pt(P(i))=i;
/* DPC */ for( unsigned int i=0;i<size();++i )
/* DPC */ if( isActive(i) ) cstMap[i].row = Pt(cstMap[i].row);
/* DPC */}
VectorXi get_cross_table(const VectorXi &values) {
IntsSet vals(values.data(), values.data() + values.size());
VectorXi cross_table = VectorXi::Zero(vals.size());
IntsSet::const_iterator val_begin = vals.begin();
IntsSet::iterator value_it;
for (int i = 0; i < values.size(); ++i) {
value_it = vals.find(values(i));
unsigned int j = std::distance(val_begin, value_it);
cross_table(j) += 1;
}
return cross_table;
}
virtual int repLabel() const {
// Count the label occurence
const int n_lbl = lbl_.maxCoeff()+1;
VectorXi lbl_cnt = VectorXi::Zero( n_lbl );
for( int i=0; i<lbl_.size(); i++ )
lbl_cnt[lbl_[i]]++;
// And return the maximum
int rep_lbl = 0;
for( int i=0; i<lbl_.size(); i++ )
if( lbl_cnt[lbl_[rep_lbl]] < lbl_cnt[lbl_[i]] )
rep_lbl = i;
return rep_lbl;
}
int HilbertSpace::getIndex(VectorXi vec) const
{
if (vec.size() != _rank)
throw std::invalid_argument("Vector size must be equal to space dimension");
int index = vec[vec.size() - 1];
int multiplier = _dimensions[_rank - 1];
for (int i = vec.size() - 2; i >= 0; --i) {
index += vec[i] * multiplier;
multiplier *= _dimensions[i];
}
return index;
}
MatrixXd slice(const MatrixXd &A, const VectorXi &R, const VectorXi &C) {
assert(R.minCoeff() >= 0);
assert(C.minCoeff() >= 0);
assert(R.maxCoeff() < A.rows());
assert(C.maxCoeff() < A.cols());
int rows = R.size();
int cols = C.size();
MatrixXd B(rows,cols);
for ( int i = 0 ; i < rows ; i++ ) {
for ( int j = 0 ; j < cols ; j++ ) {
B(i,j) = A(R(i),C(j));
} }
return (B);
}
void CMT::STM::initialize(const MatrixXd& input, const MatrixXd& output) {
if(input.rows() != dimIn() || output.rows() != dimOut())
throw Exception("Data has wrong dimensionality.");
if(input.cols() != output.cols())
throw Exception("The number of inputs and outputs should be the same.");
Array<bool, 1, Dynamic> spikes = output.array() > 0.5;
int numSpikes = spikes.sum();
if(numSpikes > dimInNonlinear() && dimInNonlinear() > 0) {
mSharpness = 1.;
MatrixXd inputNonlinear = input.topRows(dimInNonlinear());
MatrixXd inputs1(inputNonlinear.rows(), numSpikes);
MatrixXd inputs0(inputNonlinear.rows(), spikes.size() - numSpikes);
// separate data into spike-triggered and non-spike-triggered stimuli
for(int i = 0, i0 = 0, i1 = 0; i < spikes.size(); ++i)
if(spikes[i])
inputs1.col(i1++) = inputNonlinear.col(i);
else
inputs0.col(i0++) = inputNonlinear.col(i);
// spike-triggered/non-spike-triggered mean and precision
VectorXd m1 = inputs1.rowwise().mean();
VectorXd m0 = inputs0.rowwise().mean();
MatrixXd S1 = covariance(inputs1).inverse();
MatrixXd S0 = covariance(inputs0).inverse();
// parameters of a quadratic model
MatrixXd K = (S0 - S1) / 2.;
VectorXd w = S1 * m1 - S0 * m0;
double p = static_cast<float>(numSpikes) / output.cols();
double a = 0.5 * (m0.transpose() * S0 * m0)(0, 0) - 0.5 * (m1.transpose() * S1 * m1)(0, 0)
+ 0.5 * logDetPD(S1) - 0.5 * logDetPD(S0) + log(p) - log(1. - p)
- log(mNumComponents);
// decompose matrix into eigenvectors
SelfAdjointEigenSolver<MatrixXd> eigenSolver(K);
VectorXd eigVals = eigenSolver.eigenvalues();
MatrixXd eigVecs = eigenSolver.eigenvectors();
VectorXi indices = argSort(eigVals.array().abs());
// use most informative eigenvectors as features
for(int i = 0; i < mNumFeatures && i < indices.size(); ++i) {
int j = indices[i];
mWeights.col(i).setConstant(eigVals[j]);
mFeatures.col(i) = eigVecs.col(j);
}
mWeights = mWeights.array() * (0.5 + 0.5 * ArrayXXd::Random(mNumComponents, mNumFeatures).abs());
mPredictors.rowwise() = w.transpose();
mPredictors += sampleNormal(mNumComponents, mDimInNonlinear).matrix() * log(mNumComponents) / 10.;
mBiases.setConstant(a);
mBiases += VectorXd::Random(mNumComponents) * log(mNumComponents) / 100.;
}
if(dimInLinear() > 0)
mLinearPredictor = input.bottomRows(dimInLinear()) * output.transpose() / numSpikes;
}
void FFOgkBasis(
const MatrixXd& xi,
const int& calcM,
const int& intercept,
VectorXi& warn,
const int& h,
VectorXi& dIn,
int w3
){
double (*pFo[])(Ref<VectorXd>,int)={&qn,&scaleTau2};
double (*qFo[])(Ref<VectorXd>,int)={&Fmedian,&scaleTau2};
const int p=xi.cols(),n=xi.rows(),h0=(n+1)/2;
double b1=0.0,b2=0.0;
const double tol=1e-8;
int i,j;
MatrixXd x=xi;
RowVectorXd lamba(p);
MatrixXd x2=x;
if(intercept){
for(i=0;i<p;i++) lamba(i)=qCalc(x2.col(i),1,qFo[calcM]);
x.rowwise()-=lamba;
}
for(i=0;i<p;i++) lamba(i)=pCalc(x2.col(i),0,pFo[calcM]);
for(i=0;i<p;i++) warn(i)=(lamba(i)<tol)?1:0;
i=warn.sum();
if(i>0) return;
for(i=0;i<p;i++) x.col(i).array()/=lamba(i);
VectorXd dvec1=VectorXd::Ones(p);
MatrixXd U=dvec1.asDiagonal();
VectorXd sYi(n);
VectorXd sYj(n);
VectorXd dY(n);
for(i=0;i<p;++i){
sYi=x.col(i);
for(j=0;j<i;++j){
sYj=x.col(j);
dY=sYi+sYj;
b1=pCalc(dY,0,pFo[calcM]);
b1*=b1;
dY=sYi-sYj;
b2=pCalc(dY,0,pFo[calcM]);
b2*=b2;
U(i,j)=0.25*(b1-b2);
U(j,i)=U(i,j);
}
}
JacobiSVD<MatrixXd> svd(U,ComputeThinV);
x2=x*svd.matrixV();
for(i=0;i<p;i++) lamba(i)=pCalc(x2.col(i),0,pFo[calcM]);
for(i=0;i<p;i++) warn(i)=(lamba(i)<tol)?1:0;
i=warn.sum();
if(i>0) return;
for(i=0;i<p;i++) x2.col(i).array()/=lamba(i);
dY=x2.array().abs2().rowwise().sum();
dIn.setLinSpaced(n,0,n-1);
std::nth_element(dIn.data(),dIn.data()+h,dIn.data()+dIn.size(),IdLess(dY.data()));
cov_CStep(dIn,x,h,h,w3);
return;
}
inline Matrix<T, Dynamic, 1> pick(const Matrix<T, Dynamic, 1>& x, const VectorXi& ind)
{
Matrix<T, Dynamic, 1> y(ind.size());
for(unsigned i = 0; i < ind.size(); ++i)
y(i) = x(ind(i));
return y;
}
VectorXi IOUSet::computeTree(const VectorXb & s) const {
VectorXi r = VectorXi::Zero(parent_.size());
std::copy( s.data(), s.data()+s.size(), r.data() );
for( int i=0; i<r.size(); i++ )
if( parent_[i] >= 0 )
r[ parent_[i] ] += r[i];
return r;
}
void cov_CStep(
VectorXi& dIn,
MatrixXd& x,
const int h,
const int h0,
int w3
){
const int n=x.rows(),p=x.cols();
double w1,w0;
int w2=1,i;
MatrixXd xSub(h,p);
for(i=0;i<h0;i++) xSub.row(i)=x.row(dIn(i));
RowVectorXd xSub_mean(p);
xSub_mean=xSub.topRows(h0).colwise().mean();
xSub.topRows(h0).rowwise()-=xSub_mean;
x.rowwise()-=xSub_mean;
MatrixXd Sig(p,p);
//Sig=xSub.topRows(h0).adjoint()*xSub.topRows(h0);
Sig.setZero().selfadjointView<Lower>().rankUpdate(xSub.topRows(h0).transpose());
Sig.array()/=(double)(h0-1);
LDLT<MatrixXd> chol=Sig.ldlt();
MatrixXd b=MatrixXd::Identity(p,p);
chol.solveInPlace(b);
w1=chol.vectorD().array().minCoeff();
VectorXd dP(n);
if(w1>1e-6){
w1=std::numeric_limits<double>::max();
dP=((x*b).cwiseProduct(x)).rowwise().sum();
} else {
w2=0;
w3=0;
w1=chol.vectorD().array().log().sum()*2.00;
}
while(w2){
dIn.setLinSpaced(n,0,n-1);
std::nth_element(dIn.data(),dIn.data()+h,dIn.data()+dIn.size(),IdLess(dP.data()));
for(i=0;i<h;i++) xSub.row(i)=x.row(dIn(i));
xSub_mean=xSub.colwise().mean();
xSub.rowwise()-=xSub_mean;
x.rowwise()-=xSub_mean;
//Sig=xSub.adjoint()*xSub;
Sig.setZero().selfadjointView<Lower>().rankUpdate(xSub.transpose());
Sig.array()/=(double)(h-1);
chol=Sig.ldlt();
b=MatrixXd::Identity(p,p);
chol.solveInPlace(b);
if(chol.vectorD().array().minCoeff()>1e-6){
w0=w1;
w1=chol.vectorD().array().log().sum()*2.00;
dP=((x*b).cwiseProduct(x)).rowwise().sum();
(w0-w1<1e-3)?(w2=0):(w2=1);
} else {
w2=0;
w3=0;
}
}
}
std::string Proector::_vecToLabel(VectorXi vec)
{
std::string v = "";
for (int i = 0; i < vec.size(); ++i) {
if (i != 0) v += ",";
v += i_to_string(vec[i]);
}
return "|" + v + "><" + v + "|";
}
VectorXd slice(const VectorXd &A, const VectorXi &I) {
assert(I.minCoeff() >= 0);
assert(I.maxCoeff() < A.size());
int size = I.size();
VectorXd B(size);
for ( int i = 0 ; i < size ; i++ ) {
B(i) = A(I(i));
}
return (B);
}
// Finds the n occurences of the body index in the original list of m contact
// indexes
// INPUTS:
// idxList: (m x 1) Indexes of m bodies for possible contact pairs
// bodyIdx: the body index to search for
// OUTPUTS:
// contactIdx: the list of n indexes into idxList where bodyIdx occurred
void findContactIndexes(VectorXi const &idxList, const size_t bodyIdx,
std::vector<size_t> &contactIdx) {
size_t m = idxList.size();
contactIdx.clear();
for (size_t i = 0; i < m; i++) {
if (idxList[i] == static_cast<int>(bodyIdx)) {
contactIdx.push_back(i); // zero-based index
}
}
}
void add_submatrix(MatrixXd& M, const MatrixXd& Msub, const VectorXi& ind)
{
int k = 0;
for(int i=0;i<ind.size();i++){
if(ind(i) == 1){
M.row(i) += Msub.row(k);
k++;
}
}
}
//Finds the n occurences of the body index in the original list of m contact indexes
// INPUTS:
// idxList: (m x 1) Indexes of m bodies for possible contact pairs
// bodyIdx: the body index to search for
// OUTPUTS:
// contactIdx: the list of n indexes into idxList where bodyIdx occurred
void findContactIndexes(VectorXi const & idxList, const int bodyIdx, std::vector<int> & contactIdx)
{
int m = idxList.size();
contactIdx.clear();
for (int i = 0 ; i < m ; i++) {
if (idxList[i] == bodyIdx) {
contactIdx.push_back(i); //zero-based index
}
}
}
MatrixXi get_cross_table(const VectorXi &values, const VectorXi &classes) {
if(classes.rows() != values.rows()) {
throw ValueError("The vector of classes and values do not have same size");
}
IntsSet vals(values.data(), values.data() + values.size());
IntsSet cls(classes.data(), classes.data() + classes.size());
MatrixXi cross_table = MatrixXi::Zero(vals.size(), cls.size());
IntsSet::const_iterator cls_begin = cls.begin();
IntsSet::const_iterator val_begin = vals.begin();
IntsSet::iterator class_it;
IntsSet::iterator value_it;
for (int i = 0; i < classes.size(); ++i) {
class_it = cls.find( classes(i));
value_it = vals.find(values(i));
unsigned int j = std::distance(val_begin, value_it);
unsigned int k = std::distance(cls_begin, class_it);
cross_table(j, k) += 1;
}
return cross_table;
}
VectorXcd HilbertSpace::getBasisVector(VectorXi basisVec) const
{
if (basisVec.size() != _rank)
throw std::invalid_argument("Basis vector size must be equal to space dimension");
VectorXcd vec(_dim);
vec.setZero();
vec[getIndex(basisVec)] = 1;
return vec;
}
// Finds the n occurences of the body index in the original list of m contact
// indexes
// INPUTS:
// idxList: (m x 1) Indexes of m bodies for possible contact pairs
// bodyIdx: the body index to search for
// OUTPUTS:
// contactIdx: the list of n indexes into idxList where bodyIdx occurred
void findContactIndexes(VectorXi const &idxList, const size_t bodyIdx,
// TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
std::vector<size_t> &contactIdx) {
size_t m = idxList.size();
contactIdx.clear();
for (size_t i = 0; i < m; i++) {
if (idxList[i] == static_cast<int>(bodyIdx)) {
contactIdx.push_back(i); // zero-based index
}
}
}
inline Matrix<T, Dynamic, 1> remove(const Matrix<T, Dynamic, 1>& x, const VectorXi& ind)
{
if(ind.size() == 0)
return x;
for(unsigned i = 0; i < ind.size(); ++i)
if(ind(i) < 0 || ind(i) >= x.size())
throw std::range_error("EigenOctave: remove: error: each index must be within range of 0 <= ind(i) < x.size()");
for(unsigned i = 1; i < ind.size(); ++i)
if(ind(i) <= ind(i-1))
throw std::invalid_argument("EigenOctave: remove: error: ind must be sorted and without duplicate entries");
if(x.size() <= ind.size())
return Matrix<T, Dynamic, 1>();
Matrix<T, Dynamic, 1> y(x.size() - ind.size());
for(unsigned i = 0, k = 0; i < y.size(); ++i)
{
while(k < ind.size() && i+k == ind(k))
++k;
y(i) = x(i+k);
}
return y;
}
void MathUtilities::extractDataCCS( const SparseMatrixsc& A, VectorXi& col_ptr, VectorXi& row_ind, VectorXs& val )
{
col_ptr.resize( A.cols() + 1 );
row_ind.resize( A.nonZeros() );
val.resize( A.nonZeros() );
col_ptr(0) = 0;
for( int col = 0; col < A.outerSize(); ++col )
{
col_ptr(col+1) = col_ptr(col);
for( SparseMatrixsc::InnerIterator it(A,col); it; ++it )
{
const int row{ it.row() };
val(col_ptr(col+1)) = it.value();
row_ind(col_ptr(col+1)) = row;
++col_ptr(col+1);
}
}
assert( col_ptr( col_ptr.size() - 1 ) == row_ind.size() );
}
// compute objective function on a subset of the data
// (L1 penalized trimmed sum of squared residuals)
double objective(const VectorXd& beta, const VectorXd& residuals,
const VectorXi& subset, const double& lambda) {
// compute sum of squared residuals for subset
const int h = subset.size();
double crit = 0;
for(int i = 0; i < h; i++) {
crit += pow(residuals(subset(i)), 2);
}
// add L1 penalty on coefficients
crit += h * lambda * beta.lpNorm<1>();
// return value of objective function
return crit;
}
virtual float gain( const VectorXb& is_left ) const {
const int N = lbl_.maxCoeff()+1;
ArrayXf wl = 1e-20*ArrayXf::Ones(N), wr = 1e-20*ArrayXf::Ones(N);
for( int i=0; i<(int)lbl_.size(); i++ )
if( is_left[i] )
wl[lbl_[i]] += weight_[i];
else
wr[lbl_[i]] += weight_[i];
const float l = wl.sum(), r = wr.sum();
const float sl = score(wl/l), sr = score(wr/r);
printf("%d %d\n", (int)l, (int)r );
return score( (wl+wr)/(l+r) ) - (sl*l/(l+r) + sr*r/(l+r));
}
VectorXi EMclustering::unique(VectorXi l, VectorXi &l2)
{
unique_u.clear();
unique_index.clear();
int d = l.size();
unique_u.push_back(l(0));
bool flag;
for(int i=1;i<d;i++)
{
int size = unique_u.size();
flag = true;
for(int j=0;j<size;j++)
{
if(l(i)==unique_u[j])
{
flag = false;
break;
}
}
if(flag)
{
unique_u.push_back(l(i));
//cerr<<l(i)<<endl;
}
}//cerr<<unique_u.size()<<" "<<unique_index.size()<<endl;
for(int i=0;i<d;i++)
{
for(int j=0;j<unique_u.size();j++)
{
if(l(i)==unique_u[j])
{unique_index.push_back(j);}//cerr<<unique_u.size()<<" "<<unique_index.size()<<endl;}
}
}
/*sort(unique_index.begin(),unique_index.end(),myfunction);
sort(unique_u.begin(),unique_u.end());*/
VectorXi u(unique_u.size());//cerr<<unique_u.size()<<" "<<unique_index.size()<<endl;
for(int i=0;i<unique_u.size();i++)
{
u(i) = unique_u[i];
}//cerr<<unique_u.size()<<" "<<unique_index.size()<<endl;
for(int i=0;i<unique_index.size();i++)
{
l2(i) = unique_index[i];//cerr<<unique_u.size()<<" "<<unique_index.size()<<endl;
}
//cerr<<l2<<endl;
return u;
}
