bmrm_return_value_T svm_bmrm_solver(
bmrm_data_T* data,
float64_t* W,
float64_t TolRel,
float64_t TolAbs,
float64_t lambda,
uint32_t _BufSize,
bool cleanICP,
uint32_t cleanAfter,
float64_t K,
uint32_t Tmax,
CRiskFunction* risk_function)
{
bmrm_return_value_T bmrm = {0, 0, 0, 0, 0, 0, 0};
libqp_state_T qp_exitflag;
float64_t *b, *beta, *diag_H, sq_norm_W;
float64_t R, *subgrad, *A, QPSolverTolRel, rsum, C=1.0;
uint32_t *I, *ICPcounter, *ICPs, cntICP=0;
uint8_t S = 1;
uint32_t nDim=data->w_dim;
CTime ttime;
float64_t tstart, tstop;
float64_t *b2, *beta2, *diag_H2, *A2, *H2;
uint32_t *I2, *ICPcounter2, nCP_new=0, idx=0, idx2=0, icp_iter=0, icp_iter2=0;
int32_t idx_icp=0, idx_icp2=0;
bool flag1=true, flag2=true;
tstart=ttime.cur_time_diff(false);
BufSize=_BufSize;
QPSolverTolRel=TolRel*0.5;
H=NULL;
b=NULL;
beta=NULL;
A=NULL;
subgrad=NULL;
diag_H=NULL;
I=NULL;
ICPcounter=NULL;
ICPs=NULL;
b2=NULL;
beta2=NULL;
H2=NULL;
I2=NULL;
ICPcounter2=NULL;
A2=NULL;
diag_H2=NULL;
H=(float64_t*)LIBBMRM_CALLOC(BufSize*BufSize, sizeof(float64_t));
if (H==NULL)
{
bmrm.exitflag=-2;
goto cleanup;
}
A=(float64_t*)LIBBMRM_CALLOC(nDim*BufSize, sizeof(float64_t));
if (A==NULL)
{
bmrm.exitflag=-2;
goto cleanup;
}
b=(float64_t*)LIBBMRM_CALLOC(BufSize, sizeof(float64_t));
if (b==NULL)
{
bmrm.exitflag=-2;
goto cleanup;
}
beta=(float64_t*)LIBBMRM_CALLOC(BufSize, sizeof(float64_t));
if (beta==NULL)
{
bmrm.exitflag=-2;
goto cleanup;
}
subgrad=(float64_t*)LIBBMRM_CALLOC(nDim, sizeof(float64_t));
if (subgrad==NULL)
{
bmrm.exitflag=-2;
goto cleanup;
}
diag_H=(float64_t*)LIBBMRM_CALLOC(BufSize, sizeof(float64_t));
if (diag_H==NULL)
{
bmrm.exitflag=-2;
goto cleanup;
}
I=(uint32_t*)LIBBMRM_CALLOC(BufSize, sizeof(uint32_t));
if (I==NULL)
{
bmrm.exitflag=-2;
goto cleanup;
}
ICPcounter=(uint32_t*)LIBBMRM_CALLOC(BufSize, sizeof(uint32_t));
if (ICPcounter==NULL)
{
bmrm.exitflag=-2;
goto cleanup;
}
ICPs=(uint32_t*)LIBBMRM_CALLOC(BufSize, sizeof(uint32_t));
if (ICPs==NULL)
{
bmrm.exitflag=-2;
goto cleanup;
}
/* Temporary buffers for ICP removal */
b2=(float64_t*)LIBBMRM_CALLOC(BufSize, sizeof(float64_t));
beta2=(float64_t*)LIBBMRM_CALLOC(BufSize, sizeof(float64_t));
ICPcounter2=(uint32_t*)LIBBMRM_CALLOC(BufSize, sizeof(uint32_t));
I2=(uint32_t*)LIBBMRM_CALLOC(BufSize, sizeof(uint32_t));
diag_H2=(float64_t*)LIBBMRM_CALLOC(BufSize, sizeof(float64_t));
A2=(float64_t*)LIBBMRM_CALLOC(nDim*BufSize, sizeof(float64_t));
H2=(float64_t*)LIBBMRM_CALLOC(BufSize*BufSize, sizeof(float64_t));
if (b2==NULL || beta2==NULL || ICPcounter2==NULL || I2==NULL || diag_H2==NULL || A2==NULL || H2==NULL)
{
bmrm.exitflag=-2;
}
/* Iinitial solution */
risk_function->risk(data, &R, subgrad, W);
bmrm.nCP=0;
bmrm.nIter=0;
bmrm.exitflag=0;
b[0]=-R;
LIBBMRM_MEMCPY(A, subgrad, nDim*sizeof(float64_t));
/* Compute initial value of Fp, Fd, assuming that W is zero vector */
sq_norm_W=0;
bmrm.Fp=R+0.5*lambda*sq_norm_W;
bmrm.Fd=-LIBBMRM_PLUS_INF;
tstop=ttime.cur_time_diff(false);
/* Verbose output */
SG_SPRINT("%4d: tim=%.3lf, Fp=%lf, Fd=%lf, R=%lf\n",
bmrm.nIter, tstop-tstart, bmrm.Fp, bmrm.Fd, R);
/* main loop */
while (bmrm.exitflag==0)
{
tstart=ttime.cur_time_diff(false);
bmrm.nIter++;
/* Update H */
if (bmrm.nCP>0)
{
for (uint32_t i=0; i<bmrm.nCP; ++i)
{
rsum=0.0;
for (uint32_t j=0; j<nDim; ++j)
{
rsum+=A[LIBBMRM_INDEX(j, i, nDim)]*A[LIBBMRM_INDEX(j, bmrm.nCP, nDim)];
}
H[LIBBMRM_INDEX(i, bmrm.nCP, BufSize)]=rsum/lambda;
}
for (uint32_t i=0; i<bmrm.nCP; ++i)
{
H[LIBBMRM_INDEX(bmrm.nCP, i, BufSize)]=H[LIBBMRM_INDEX(i, bmrm.nCP, BufSize)];
}
}
H[LIBBMRM_INDEX(bmrm.nCP, bmrm.nCP, BufSize)]=0.0;
for (uint32_t i=0; i<nDim; ++i)
H[LIBBMRM_INDEX(bmrm.nCP, bmrm.nCP, BufSize)]+=A[LIBBMRM_INDEX(i, bmrm.nCP, nDim)]*A[LIBBMRM_INDEX(i, bmrm.nCP, nDim)]/lambda;
diag_H[bmrm.nCP]=H[LIBBMRM_INDEX(bmrm.nCP, bmrm.nCP, BufSize)];
I[bmrm.nCP]=1;
bmrm.nCP++;
/* call QP solver */
qp_exitflag = libqp_splx_solver(&get_col, diag_H, b, &C, I, &S, beta,
bmrm.nCP, QPSolverMaxIter, 0.0, QPSolverTolRel, -LIBBMRM_PLUS_INF, 0);
bmrm.qp_exitflag=qp_exitflag.exitflag;
/* Update ICPcounter (add one to unused and reset used) + compute number of active CPs*/
bmrm.nzA=0;
for (uint32_t aaa=0; aaa<bmrm.nCP; ++aaa)
{
if (beta[aaa]>epsilon)
{
bmrm.nzA+=1;
ICPcounter[aaa]=0;
} else {
ICPcounter[aaa]+=1;
}
}
/* W update */
for (uint32_t i=0; i<nDim; ++i)
{
rsum = 0.0;
for (uint32_t j=0; j<bmrm.nCP; ++j)
{
rsum += A[LIBBMRM_INDEX(i, j, nDim)]*beta[j];
}
W[i] = -rsum/lambda;
}
/* risk and subgradient computation */
risk_function->risk(data, &R, subgrad, W);
LIBBMRM_MEMCPY(A+bmrm.nCP*nDim, subgrad, nDim*sizeof(float64_t));
b[bmrm.nCP]=-R;
for (uint32_t j=0; j<nDim; ++j)
b[bmrm.nCP]+=subgrad[j]*W[j];
sq_norm_W = 0;
for (uint32_t j=0; j<nDim; ++j)
sq_norm_W+=W[j]*W[j];
bmrm.Fp=R+0.5*lambda*sq_norm_W;
bmrm.Fd=-qp_exitflag.QP;
/* Stopping conditions */
if (bmrm.Fp - bmrm.Fd <= TolRel*LIBBMRM_ABS(bmrm.Fp)) bmrm.exitflag=1;
if (bmrm.Fp - bmrm.Fd <= TolAbs) bmrm.exitflag=2;
if (bmrm.nCP >= BufSize) bmrm.exitflag=-1;
tstop=ttime.cur_time_diff(false);
/* Verbose output */
SG_SPRINT("%4d: tim=%.3lf, Fp=%lf, Fd=%lf, (Fp-Fd)=%lf, (Fp-Fd)/Fp=%lf, R=%lf, nCP=%d, nzA=%d\n",
bmrm.nIter, tstop-tstart, bmrm.Fp, bmrm.Fd, bmrm.Fp-bmrm.Fd, (bmrm.Fp-bmrm.Fd)/bmrm.Fp, R, bmrm.nCP, bmrm.nzA);
/* Check size of Buffer */
if (bmrm.nCP>=BufSize)
{
bmrm.exitflag=-2;
SG_SERROR("Buffer exceeded.\n");
}
/* Inactive Cutting Planes (ICP) removal */
if (cleanICP)
{
/* find ICP */
cntICP = 0;
for (uint32_t aaa=0; aaa<bmrm.nCP; ++aaa)
if (ICPcounter[aaa]>=cleanAfter)
{
ICPs[cntICP++]=aaa;
}
/* do ICP */
if (cntICP > 0)
{
nCP_new=bmrm.nCP-cntICP;
idx=0;
idx2=0;
icp_iter=0;
icp_iter2=0;
idx_icp=ICPs[icp_iter];
idx_icp2=ICPs[icp_iter2];
flag1=true; flag2=true;
for (uint32_t i=0; i<bmrm.nCP; ++i)
{
if ((int32_t)i != idx_icp)
{
b2[idx]=b[i];
beta2[idx]=beta[i];
ICPcounter2[idx]=ICPcounter[i];
I2[idx]=I[i];
diag_H2[idx]=diag_H[i];
LIBBMRM_MEMCPY(A2+idx*nDim, A+i*nDim, nDim*sizeof(float64_t));
idx2=0;
icp_iter2=0;
idx_icp2=ICPs[icp_iter2];
flag2=true;
for (uint32_t j=0; j<bmrm.nCP; ++j)
{
if ((int32_t)j != idx_icp2)
{
H2[LIBBMRM_INDEX(idx, idx2, BufSize)]=H[LIBBMRM_INDEX(i, j, BufSize)];
idx2++;
} else {
if (flag2 && icp_iter2+1 < cntICP)
{
idx_icp2=ICPs[++icp_iter2];
} else {
flag2=false;
idx_icp2=-1;
}
}
}
idx++;
} else {
if (flag1 && icp_iter+1 < cntICP)
{
idx_icp=ICPs[++icp_iter];
} else {
flag1=false;
idx_icp=-1;
}
}
}
/* copy data from tmps back to original */
LIBBMRM_MEMCPY(b, b2, nCP_new*sizeof(float64_t));
LIBBMRM_MEMCPY(beta, beta2, nCP_new*sizeof(float64_t));
LIBBMRM_MEMCPY(ICPcounter, ICPcounter2, nCP_new*sizeof(uint32_t));
LIBBMRM_MEMCPY(I, I2, nCP_new*sizeof(uint32_t));
LIBBMRM_MEMCPY(diag_H, diag_H2, nCP_new*sizeof(float64_t));
LIBBMRM_MEMCPY(A, A2, nDim*nCP_new*sizeof(float64_t));
for (uint32_t i=0; i<nCP_new; ++i)
for (uint32_t j=0; j<nCP_new; ++j)
H[LIBBMRM_INDEX(i, j, BufSize)]=H2[LIBBMRM_INDEX(i, j, BufSize)];
bmrm.nCP=nCP_new;
}
}
} /* end of main loop */
cleanup:
LIBBMRM_FREE(H);
LIBBMRM_FREE(b);
LIBBMRM_FREE(beta);
LIBBMRM_FREE(A);
LIBBMRM_FREE(subgrad);
LIBBMRM_FREE(diag_H);
LIBBMRM_FREE(I);
LIBBMRM_FREE(ICPcounter);
LIBBMRM_FREE(ICPs);
LIBBMRM_FREE(H2);
LIBBMRM_FREE(b2);
LIBBMRM_FREE(beta2);
LIBBMRM_FREE(A2);
LIBBMRM_FREE(diag_H2);
LIBBMRM_FREE(I2);
LIBBMRM_FREE(ICPcounter2);
return(bmrm);
}
bmrm_return_value_T svm_bmrm_solver(
CStructuredModel* model,
float64_t* W,
float64_t TolRel,
float64_t TolAbs,
float64_t _lambda,
uint32_t _BufSize,
bool cleanICP,
uint32_t cleanAfter,
float64_t K,
uint32_t Tmax,
bool verbose)
{
bmrm_return_value_T bmrm;
libqp_state_T qp_exitflag={0, 0, 0, 0};
float64_t *b, *beta, *diag_H, *prevW;
float64_t R, *subgrad, *A, QPSolverTolRel, C=1.0, wdist=0.0;
floatmax_t rsum, sq_norm_W, sq_norm_Wdiff=0.0;
uint32_t *I;
uint8_t S=1;
uint32_t nDim=model->get_dim();
CTime ttime;
float64_t tstart, tstop;
bmrm_ll *CPList_head, *CPList_tail, *cp_ptr, *cp_ptr2, *cp_list=NULL;
float64_t *A_1=NULL, *A_2=NULL;
bool *map=NULL;
tstart=ttime.cur_time_diff(false);
BufSize=_BufSize;
QPSolverTolRel=1e-9;
H=NULL;
b=NULL;
beta=NULL;
A=NULL;
subgrad=NULL;
diag_H=NULL;
I=NULL;
prevW=NULL;
H= (float64_t*) LIBBMRM_CALLOC(BufSize*BufSize, sizeof(float64_t));
if (H==NULL)
{
bmrm.exitflag=-2;
goto cleanup;
}
A= (float64_t*) LIBBMRM_CALLOC(nDim*BufSize, sizeof(float64_t));
if (A==NULL)
{
bmrm.exitflag=-2;
goto cleanup;
}
b= (float64_t*) LIBBMRM_CALLOC(BufSize, sizeof(float64_t));
if (b==NULL)
{
bmrm.exitflag=-2;
goto cleanup;
}
beta= (float64_t*) LIBBMRM_CALLOC(BufSize, sizeof(float64_t));
if (beta==NULL)
{
bmrm.exitflag=-2;
goto cleanup;
}
subgrad= (float64_t*) LIBBMRM_CALLOC(nDim, sizeof(float64_t));
if (subgrad==NULL)
{
bmrm.exitflag=-2;
goto cleanup;
}
diag_H= (float64_t*) LIBBMRM_CALLOC(BufSize, sizeof(float64_t));
if (diag_H==NULL)
{
bmrm.exitflag=-2;
goto cleanup;
}
I= (uint32_t*) LIBBMRM_CALLOC(BufSize, sizeof(uint32_t));
if (I==NULL)
{
bmrm.exitflag=-2;
goto cleanup;
}
ICP_stats icp_stats;
icp_stats.maxCPs = BufSize;
icp_stats.ICPcounter= (uint32_t*) LIBBMRM_CALLOC(BufSize, sizeof(uint32_t));
if (icp_stats.ICPcounter==NULL)
{
bmrm.exitflag=-2;
goto cleanup;
}
icp_stats.ICPs= (float64_t**) LIBBMRM_CALLOC(BufSize, sizeof(float64_t*));
if (icp_stats.ICPs==NULL)
{
bmrm.exitflag=-2;
goto cleanup;
}
icp_stats.ACPs= (uint32_t*) LIBBMRM_CALLOC(BufSize, sizeof(uint32_t));
if (icp_stats.ACPs==NULL)
{
bmrm.exitflag=-2;
goto cleanup;
}
/* Temporary buffers for ICP removal */
icp_stats.H_buff= (float64_t*) LIBBMRM_CALLOC(BufSize*BufSize, sizeof(float64_t));
if (icp_stats.H_buff==NULL)
{
bmrm.exitflag=-2;
goto cleanup;
}
map= (bool*) LIBBMRM_CALLOC(BufSize, sizeof(bool));
if (map==NULL)
{
bmrm.exitflag=-2;
goto cleanup;
}
memset( (bool*) map, true, BufSize);
cp_list= (bmrm_ll*) LIBBMRM_CALLOC(1, sizeof(bmrm_ll));
if (cp_list==NULL)
{
bmrm.exitflag=-2;
goto cleanup;
}
prevW= (float64_t*) LIBBMRM_CALLOC(nDim, sizeof(float64_t));
if (prevW==NULL)
{
bmrm.exitflag=-2;
goto cleanup;
}
bmrm.hist_Fp = SGVector< float64_t >(BufSize);
bmrm.hist_Fd = SGVector< float64_t >(BufSize);
bmrm.hist_wdist = SGVector< float64_t >(BufSize);
/* Iinitial solution */
R=model->risk(subgrad, W);
bmrm.nCP=0;
bmrm.nIter=0;
bmrm.exitflag=0;
b[0]=-R;
/* Cutting plane auxiliary double linked list */
LIBBMRM_MEMCPY(A, subgrad, nDim*sizeof(float64_t));
map[0]=false;
cp_list->address=&A[0];
cp_list->idx=0;
cp_list->prev=NULL;
cp_list->next=NULL;
CPList_head=cp_list;
CPList_tail=cp_list;
/* Compute initial value of Fp, Fd, assuming that W is zero vector */
sq_norm_W=0;
bmrm.Fp=R+0.5*_lambda*sq_norm_W;
bmrm.Fd=-LIBBMRM_PLUS_INF;
tstop=ttime.cur_time_diff(false);
/* Verbose output */
if (verbose)
SG_SPRINT("%4d: tim=%.3lf, Fp=%lf, Fd=%lf, R=%lf\n",
bmrm.nIter, tstop-tstart, bmrm.Fp, bmrm.Fd, R);
/* store Fp, Fd and wdist history */
bmrm.hist_Fp[0]=bmrm.Fp;
bmrm.hist_Fd[0]=bmrm.Fd;
bmrm.hist_wdist[0]=0.0;
/* main loop */
while (bmrm.exitflag==0)
{
tstart=ttime.cur_time_diff(false);
bmrm.nIter++;
/* Update H */
if (bmrm.nCP>0)
{
A_2=get_cutting_plane(CPList_tail);
cp_ptr=CPList_head;
for (uint32_t i=0; i<bmrm.nCP; ++i)
{
A_1=get_cutting_plane(cp_ptr);
cp_ptr=cp_ptr->next;
rsum= SGVector<float64_t>::dot(A_1, A_2, nDim);
H[LIBBMRM_INDEX(bmrm.nCP, i, BufSize)]
= H[LIBBMRM_INDEX(i, bmrm.nCP, BufSize)]
= rsum/_lambda;
}
}
A_2=get_cutting_plane(CPList_tail);
rsum = SGVector<float64_t>::dot(A_2, A_2, nDim);
H[LIBBMRM_INDEX(bmrm.nCP, bmrm.nCP, BufSize)]=rsum/_lambda;
diag_H[bmrm.nCP]=H[LIBBMRM_INDEX(bmrm.nCP, bmrm.nCP, BufSize)];
I[bmrm.nCP]=1;
bmrm.nCP++;
beta[bmrm.nCP]=0.0; // [beta; 0]
#if 0
/* TODO: scaling...*/
float64_t scale = SGVector<float64_t>::max(diag_H, BufSize)/(1000.0*_lambda);
SGVector<float64_t> sb(bmrm.nCP);
sb.zero();
sb.vec1_plus_scalar_times_vec2(sb.vector, 1/scale, b, bmrm.nCP);
SGVector<float64_t> sh(bmrm.nCP);
sh.zero();
sb.vec1_plus_scalar_times_vec2(sh.vector, 1/scale, diag_H, bmrm.nCP);
qp_exitflag =
libqp_splx_solver(&get_col, sh.vector, sb.vector, &C, I, &S, beta,
bmrm.nCP, QPSolverMaxIter, 0.0, QPSolverTolRel, -LIBBMRM_PLUS_INF, 0);
#else
/* call QP solver */
qp_exitflag=libqp_splx_solver(&get_col, diag_H, b, &C, I, &S, beta,
bmrm.nCP, QPSolverMaxIter, 0.0, QPSolverTolRel, -LIBBMRM_PLUS_INF, 0);
#endif
bmrm.qp_exitflag=qp_exitflag.exitflag;
/* Update ICPcounter (add one to unused and reset used)
* + compute number of active CPs */
bmrm.nzA=0;
for (uint32_t aaa=0; aaa<bmrm.nCP; ++aaa)
{
if (beta[aaa]>epsilon)
{
++bmrm.nzA;
icp_stats.ICPcounter[aaa]=0;
}
else
{
icp_stats.ICPcounter[aaa]+=1;
}
}
/* W update */
memset(W, 0, sizeof(float64_t)*nDim);
cp_ptr=CPList_head;
for (uint32_t j=0; j<bmrm.nCP; ++j)
{
A_1=get_cutting_plane(cp_ptr);
cp_ptr=cp_ptr->next;
SGVector<float64_t>::vec1_plus_scalar_times_vec2(W, -beta[j]/_lambda, A_1, nDim);
}
/* risk and subgradient computation */
R = model->risk(subgrad, W);
add_cutting_plane(&CPList_tail, map, A,
find_free_idx(map, BufSize), subgrad, nDim);
sq_norm_W=SGVector<float64_t>::dot(W, W, nDim);
b[bmrm.nCP]=SGVector<float64_t>::dot(subgrad, W, nDim) - R;
sq_norm_Wdiff=0.0;
for (uint32_t j=0; j<nDim; ++j)
{
sq_norm_Wdiff+=(W[j]-prevW[j])*(W[j]-prevW[j]);
}
bmrm.Fp=R+0.5*_lambda*sq_norm_W;
bmrm.Fd=-qp_exitflag.QP;
wdist=CMath::sqrt(sq_norm_Wdiff);
/* Stopping conditions */
if (bmrm.Fp - bmrm.Fd <= TolRel*LIBBMRM_ABS(bmrm.Fp))
bmrm.exitflag=1;
if (bmrm.Fp - bmrm.Fd <= TolAbs)
bmrm.exitflag=2;
if (bmrm.nCP >= BufSize)
bmrm.exitflag=-1;
tstop=ttime.cur_time_diff(false);
/* Verbose output */
if (verbose)
SG_SPRINT("%4d: tim=%.3lf, Fp=%lf, Fd=%lf, (Fp-Fd)=%lf, (Fp-Fd)/Fp=%lf, R=%lf, nCP=%d, nzA=%d, QPexitflag=%d\n",
bmrm.nIter, tstop-tstart, bmrm.Fp, bmrm.Fd, bmrm.Fp-bmrm.Fd,
(bmrm.Fp-bmrm.Fd)/bmrm.Fp, R, bmrm.nCP, bmrm.nzA, qp_exitflag.exitflag);
/* Keep Fp, Fd and w_dist history */
bmrm.hist_Fp[bmrm.nIter]=bmrm.Fp;
bmrm.hist_Fd[bmrm.nIter]=bmrm.Fd;
bmrm.hist_wdist[bmrm.nIter]=wdist;
/* Check size of Buffer */
if (bmrm.nCP>=BufSize)
{
bmrm.exitflag=-2;
SG_SERROR("Buffer exceeded.\n");
}
/* keep W (for wdist history track) */
LIBBMRM_MEMCPY(prevW, W, nDim*sizeof(float64_t));
/* Inactive Cutting Planes (ICP) removal */
if (cleanICP)
{
clean_icp(&icp_stats, bmrm, &CPList_head, &CPList_tail, H, diag_H, beta, map, cleanAfter, b, I);
}
} /* end of main loop */
bmrm.hist_Fp.resize_vector(bmrm.nIter);
bmrm.hist_Fd.resize_vector(bmrm.nIter);
bmrm.hist_wdist.resize_vector(bmrm.nIter);
cp_ptr=CPList_head;
while(cp_ptr!=NULL)
{
cp_ptr2=cp_ptr;
cp_ptr=cp_ptr->next;
LIBBMRM_FREE(cp_ptr2);
cp_ptr2=NULL;
}
cp_list=NULL;
cleanup:
LIBBMRM_FREE(H);
LIBBMRM_FREE(b);
LIBBMRM_FREE(beta);
LIBBMRM_FREE(A);
LIBBMRM_FREE(subgrad);
LIBBMRM_FREE(diag_H);
LIBBMRM_FREE(I);
LIBBMRM_FREE(icp_stats.ICPcounter);
LIBBMRM_FREE(icp_stats.ICPs);
LIBBMRM_FREE(icp_stats.ACPs);
LIBBMRM_FREE(icp_stats.H_buff);
LIBBMRM_FREE(map);
LIBBMRM_FREE(prevW);
if (cp_list)
LIBBMRM_FREE(cp_list);
return(bmrm);
}
/* -------------------------------------------------------------
MEX main function.
------------------------------------------------------------- */
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[] )
{
int verb;
uint32_t MaxIter;
uint32_t *vec_I;
uint32_t nConstr;
uint8_t *vec_S;
double *vec_x;
double TolRel;
double TolAbs;
double QP_TH;
double *vec_x0;
double *vec_f;
double *vec_b;
double *diag_H;
long i ;
libqp_state_T state;
double *tmp;
/*-------------------------------------------------------------------
Get input arguments
------------------------------------------------------------------- */
if( nrhs != 12) mexErrMsgTxt("Incorrect number of input arguments.");
mat_H = (mxArray*)prhs[0];
nVar = mxGetM(prhs[0]);
diag_H = mxGetPr(prhs[1]);
vec_f = mxGetPr(prhs[2]);
vec_b = (double*)mxGetPr(prhs[3]);
vec_I = (uint32_t*)mxGetPr(prhs[4]);
vec_S = (uint8_t*)mxGetPr(prhs[5]);
nConstr = LIBQP_MAX(mxGetN(prhs[3]),mxGetM(prhs[3]));
vec_x0 = mxGetPr(prhs[6]);
MaxIter = mxIsInf( mxGetScalar(prhs[7])) ? 0xFFFFFFFF : (uint32_t)mxGetScalar(prhs[7]);
TolAbs = mxGetScalar(prhs[8]);
TolRel = mxGetScalar(prhs[9]);
QP_TH = mxGetScalar(prhs[10]);
verb = (int)mxGetScalar(prhs[11]);
/* print input setting if required */
if( verb > 0 ) {
mexPrintf("Settings of LIBQP_SSVM solver:\n");
mexPrintf("MaxIter : %u\n", MaxIter );
mexPrintf("TolAbs : %f\n", TolAbs );
mexPrintf("TolRel : %f\n", TolRel );
mexPrintf("QP_TH : %f\n", QP_TH );
mexPrintf("nVar : %u\n", nVar );
mexPrintf("nConstr : %u\n", nConstr );
}
/*-------------------------------------------------------------------
Inicialization
------------------------------------------------------------------- */
/* create solution vector x [nVar x 1] */
plhs[0] = mxCreateDoubleMatrix(nVar,1,mxREAL);
vec_x = mxGetPr(plhs[0]);
memcpy( vec_x, vec_x0, sizeof(double)*nVar );
/*-------------------------------------------------------------------
Call the QP solver.
-------------------------------------------------------------------*/
if( mxIsSparse(mat_H)== true )
{
col_buf0 = mxCalloc(nVar,sizeof(double));
if( col_buf0 == NULL)
mexErrMsgTxt("Cannot allocate memory for col_buf0.");
col_buf1 = mxCalloc(nVar,sizeof(double));
if( col_buf1 == NULL)
mexErrMsgTxt("Cannot allocate memory for col_buf1.");
/* tmp = get_col_sparse(nVar-1);
for(i=0; i<nVar; i++)
{
mexPrintf("%d: %f\n",i,tmp[i]);
vec_x[i] = tmp[i];
}
*/
state = libqp_splx_solver(&get_col_sparse, diag_H, vec_f, vec_b, vec_I, vec_S, vec_x, nVar,
MaxIter, TolAbs, TolRel, QP_TH, NULL);
mxFree(col_buf0);
mxFree(col_buf1);
}
else
{
/* tmp = get_col(nVar-1);
for(i=0; i<nVar; i++)
{
mexPrintf("%d: %f\n",i,tmp[i]);
vec_x[i] = tmp[i];
}
*/
state = libqp_splx_solver(&get_col, diag_H, vec_f, vec_b, vec_I, vec_S, vec_x, nVar,
MaxIter, TolAbs, TolRel, QP_TH, NULL);
}
/*-------------------------------------------------------------------
Set output arguments
[x,QP,QD,exitflag,nIter] = libqp_splx_mex(...)
------------------------------------------------------------------- */
plhs[1] = mxCreateDoubleMatrix(1,1,mxREAL);
*(mxGetPr(plhs[1])) = (double)state.QP;
plhs[2] = mxCreateDoubleMatrix(1,1,mxREAL);
*(mxGetPr(plhs[2])) = (double)state.QD;
plhs[3] = mxCreateDoubleMatrix(1,1,mxREAL);
*(mxGetPr(plhs[3])) = (double)state.exitflag;
plhs[4] = mxCreateDoubleMatrix(1,1,mxREAL);
*(mxGetPr(plhs[4])) = (double)state.nIter;
return;
}
int main(int argc, char *argv[])
{
FILE *input_fid = NULL;
FILE *output_fid = NULL;
char *input_fname;
char *output_fname;
int nMatrices;
int i, j, idx;
mtf_matrix_t *matrix_array = NULL;
uint32_t nConstr;
double *diag_H = NULL;
double *vec_f = NULL;
double *vec_b = NULL;
uint32_t *vec_I = NULL;
uint8_t *vec_S = NULL;
double *vec_x = NULL;
libqp_state_T exitflag;
uint32_t MaxIter = 0xFFFFFFFF;
double TolAbs = 0.0;
double TolRel = 1e-9;
double QP_TH = -LIBQP_PLUS_INF;
double load_time = 0;
double solver_time = 0;
double total_time = get_time();
if(argc != 3)
{
fprintf(stderr,"Usage: qpsplx input_file output_file\n");
return(0);
}
input_fname = argv[1];
output_fname = argv[2];
fprintf(stdout,"Input file: %s\n", input_fname);
fprintf(stdout,"Output file: %s\n", output_fname);
input_fid = fopen(input_fname, "r");
if(input_fid == NULL)
{
perror("fopen error: ");
fprintf(stderr,"Cannot open input file.");
return(0);
}
load_time = get_time();
nMatrices = mtf_read_file(input_fid, &matrix_array);
load_time = get_time()-load_time;
if(nMatrices < 1)
{
fprintf(stderr,"Error when reading matrices from file.\n");
mtf_print_error( nMatrices );
goto cleanup;
}
if(nMatrices != 5)
{
fprintf(stderr,"Input file must contain five matrices/vectors.");
goto cleanup_free_matrices;
}
/* fprintf(stdout,"Number of loaded matrices: %d\n", nMatrices);
for(i=0; i < nMatrices; i++ )
{
fprintf(stdout,"name: %s nRows: %d nCols: %d\n",
matrix_array[i].name, matrix_array[i].nRows, matrix_array[i].nCols);
}
*/
/*--------------------------------------------------*/
/* Get input matrices and vectors */
/*--------------------------------------------------*/
idx = mtf_get_index("H",matrix_array,nMatrices);
if(idx < 0 )
{
fprintf(stderr,"Matrix H not found.\n");
goto cleanup_free_matrices;
}
mat_H = matrix_array[idx].value;
nVars = matrix_array[idx].nCols;
idx = mtf_get_index("f",matrix_array,nMatrices);
if(idx < 0 )
{
fprintf(stderr,"Vector f not found.\n");
goto cleanup_free_matrices;
}
vec_f = matrix_array[idx].value;
idx = mtf_get_index("b",matrix_array,nMatrices);
if(idx < 0 )
{
fprintf(stderr,"Vector b not found.\n");
goto cleanup_free_matrices;
}
vec_b = matrix_array[idx].value;
nConstr = LIBQP_MAX(matrix_array[idx].nCols,matrix_array[idx].nRows);
idx = mtf_get_index("I",matrix_array,nMatrices);
if(idx < 0 )
{
fprintf(stderr,"Vector I not found.\n");
goto cleanup_free_matrices;
}
vec_I = calloc(nVars,sizeof(uint32_t));
if(vec_I == NULL) {
fprintf(stderr,"Not enough memory.\n");
goto cleanup_free_matrices;
}
for(i=0; i < nVars; i++)
vec_I[i] = (uint32_t)matrix_array[idx].value[i];
idx = mtf_get_index("S",matrix_array,nMatrices);
if(idx < 0 )
{
fprintf(stderr,"Vector S not found.\n");
goto cleanup_free_matrices;
}
vec_S = calloc(nConstr,sizeof(uint8_t));
if(vec_S == NULL) {
fprintf(stderr,"Not enough memory.\n");
goto cleanup_free_matrices;
}
for(i=0; i < nConstr; i++)
vec_S[i] = (uint8_t)matrix_array[idx].value[i];
diag_H = calloc(nVars,sizeof(double));
if(diag_H == NULL) {
fprintf(stderr,"Not enough memory.\n");
goto cleanup_free_matrices;
}
for(i=0; i < nVars; i++)
diag_H[i] = mat_H[LIBQP_INDEX(i,i,nVars)];
/*--------------------------------------------------*/
/* Create initial feasible solution */
/*--------------------------------------------------*/
vec_x = calloc(nVars, sizeof(double));
if( vec_x == NULL )
{
fprintf(stderr,"Not enough memory.\n");
goto cleanup_free_matrices;
}
for(i=0; i < nVars; i++)
vec_x[i] = 0.0;
for(i=0; i < nConstr; i++ )
{
if(vec_S[i] == 0)
{
for(j=0; j < nVars; j++)
{
if(vec_I[j] == i+1)
{
vec_x[j] = vec_b[i];
break;
}
}
}
}
printf("nVar: %d nConstr: %d\n", nVars, nConstr);
/*--------------------------------------------------*/
/* Call the QP solver and print results */
/*--------------------------------------------------*/
printf("Solving QP..."); fflush(stdout);
solver_time = get_time();
exitflag = libqp_splx_solver(get_col_of_mat_H, diag_H, vec_f, vec_b,
vec_I, vec_S, vec_x, nVars, MaxIter,
TolAbs, TolRel, QP_TH, NULL);
solver_time = get_time()-solver_time;
printf("done.\n");
total_time= get_time() - total_time;
printf("QP: %f\n", exitflag.QP);
printf("QD: %f\n", exitflag.QD);
printf("nIter: %d\n", exitflag.nIter);
printf("exitflag: %d\n", exitflag.exitflag);
printf("load_time: %f\n", load_time);
printf("solver_time: %f\n", solver_time);
printf("total_time: %f\n", total_time);
/*--------------------------------------------------*/
/* Write result to file */
/*--------------------------------------------------*/
output_fid = fopen(output_fname, "w+");
if(output_fid == NULL)
{
perror("fopen error: ");
fprintf(stderr,"Cannot open input file.");
goto cleanup_free_matrices;
}
fprintf(output_fid,"6\n");
fprintf(output_fid,"QP 1 1\n");
fprintf(output_fid,"%.12f\n",exitflag.QP);
fprintf(output_fid,"QD 1 1\n");
fprintf(output_fid,"%.12f\n",exitflag.QD);
fprintf(output_fid,"nIter 1 1\n");
fprintf(output_fid,"%d\n",exitflag.nIter);
fprintf(output_fid,"exitflag 1 1\n");
fprintf(output_fid,"%d\n",exitflag.exitflag);
fprintf(output_fid,"solver_time 1 1\n");
fprintf(output_fid,"%.12f\n",solver_time);
fprintf(output_fid,"x %d 1\n", nVars);
for(i=0; i < nVars; i++ )
fprintf(output_fid,"%.12f\n", vec_x[i]);
fclose(output_fid);
cleanup_free_matrices:
free(diag_H);
free(vec_I);
free(vec_S);
free(vec_x);
for(i=0; i < nMatrices; i++ )
free(matrix_array[i].value);
cleanup:
fclose(input_fid);
return(0);
}