double bEyesObject::findEucDis()
{
double xSquare = getAbs(xLeft-xRight) * getAbs(xLeft-xRight);
double ySquare = getAbs(yLeft-yRight) * getAbs(yLeft-yRight);
double EucDis = sqrt(xSquare + ySquare);
return EucDis;
}
int sub(struct NUMBER *a,struct NUMBER *b,struct NUMBER *c)
{
int i,f_hizero = 0,h = 0,res = 0,tmp = 1;
struct NUMBER tmp_a,tmp_b;
/*値を動かすので一時的に保存*/
copyNumber(a,&tmp_a);
copyNumber(b,&tmp_b);
if(numComp(a,b) == -1)
{
copyNumber(a,&tmp_b);
copyNumber(b,&tmp_a);//a > bにしたいので
tmp = -1;
}
else
{
copyNumber(a,&tmp_a);
copyNumber(b,&tmp_b);
}
clearByZero(c);//cを0に
if(getSign(a) == 1&&getSign(b) == 1)//a,bも+の時は通常のsub
{
if(numComp(&tmp_a,&tmp_b) == 0)
return(0);//aとbが同じならcは0だから
f_hizero = firstNotZero(&tmp_a);//aは必ず大きい値なのでf_hizeroも大きい方になる
/*計算部*/
for(i = 0;i <= f_hizero;i++)
{
tmp_a.n[i] -= h;//繰り下がり分を引く
h = (tmp_a.n[i]>=tmp_b.n[i])? 0:1;
c->n[i] = h * 10 + tmp_a.n[i] - tmp_b.n[i];
}
if(f_hizero == KETA - 1&&h != 0)
res = -1;
setSign(c,tmp);
}
/*絶対値を使うので保存*/
getAbs(a,&tmp_a);
getAbs(b,&tmp_b);
if(getSign(a) == 1 && getSign(b) == -1)//aが+、bが-の時はa+b通常のadd
res = add(a,&tmp_b,c);
if(getSign(a) == -1 && getSign(b) == 1 )//aが-、bが+の時は-a-b、つまり-(a+b)、addの結果を負にする
{
res = add(b,&tmp_a,c);
setSign(c,-1);
}
if(getSign(a) == -1 && getSign(b) == -1)//aもbも-の時は-a + b、つまりb-aをすればいい。
res = sub(&tmp_b,&tmp_a,c);
return(res);
}
int sub(struct NUMBER *a,struct NUMBER *b,struct NUMBER *c)
{
int i,h=0,d=0;
struct NUMBER temp;
clearByZero(&temp);
clearByZero(c);
if(getSign(a)==1)
{
if(getSign(b)==1)
{
for(i=0;i<KETA;i++)
{
if(numComp(a,b)==1)
{
d=a->n[i]-b->n[i]-h;
h=0;
if(d<0)
{
d+=10;
h=1;
}
c->n[i]=d;
}
else
{
getAbs(b,&temp);
sub(&temp,a,c);
setSign(c,-1);
}
}
}
else if(getSign(b)==-1)
{
getAbs(b,&temp);
add(a,&temp,c);
}
}
else if(getSign(a)==-1)
{
if(getSign(b)==1)
{
getAbs(a,&temp);
add(b,&temp,c);
setSign(c,-1);
}
else if(getSign(b)==-1)
{
getAbs(b,&temp);
add(&temp,a,c);
}
}
return 0;
}
int multiple(struct NUMBER *a,struct NUMBER *b,struct NUMBER *c)
{
int i,j,h=0,tempb,tempa,e;
struct NUMBER d,mai1,mai2,temp;
clearByZero(&d);
clearByZero(c);
if(getSign(a)==1)
{
if(getSign(b)==1)
{
for(i=0;i<KETA-1;i++)
{
h=0;
clearByZero(&d);
tempb=b->n[i];
for(j=0;j<KETA;j++)
{
tempa=a->n[j];
e=tempa*tempb + h;
if(j+i<KETA)
d.n[j+i]=e%10;
h=e/10;
}
copyNumber(c,&temp);
add(&temp,&d,c);
}
}
else if(getSign(b)==-1)
{
getAbs(b,&mai1);
multiple(a,&mai1,c);
setSign(c,-1);
}
}
else if(getSign(a)==-1)
{
if(getSign(b)==1)
{
getAbs(a,&mai1);
multiple(b,&mai1,c);
setSign(c,-1);
}
else if(getSign(b)==-1)
{
getAbs(a,&mai1);
getAbs(b,&mai2);
multiple(&mai1,&mai2,c);
}
}
}
int add(struct NUMBER *a,struct NUMBER *b, struct NUMBER *c)
{
int i,e=0,d=0;
int flag=0;
struct NUMBER temp,temp2;
clearByZero(&temp);
clearByZero(&temp2);
clearByZero(c);
if(getSign(a)==1)
{
if(getSign(b)==1)
{
for(i=0;i<KETA;i++)
{
d=a->n[i]+b->n[i]+e;
if(i==KETA-1 && d>=10)
{
printf("OverFlow\n");
clearByZero(c);
flag=1;
break;
}
c->n[i]=d%10;
e=d/10;
}
}
else if(getSign(b)==-1)
{
getAbs(b,&temp);
sub(a,&temp,c);
}
}
else
{
if(getSign(b)==1){
getAbs(a,&temp);
sub(b,&temp,c);
}
else if(getSign(b)==-1)
{
getAbs(a,&temp);
getAbs(b,&temp2);
add(&temp,&temp2,c);
setSign(c,-1);
}
}
return flag;
}
int Darknet::loadNetwork(const QString &cfg, const QString &weights)
{
priv->net = parse_network_cfg((char *)qPrintable(getAbs(cfg)));
load_weights(&priv->net, (char *)qPrintable(getAbs(weights)));
set_batch_network(&priv->net, 1);
priv->l = priv->net.layers[priv->net.n-1];
priv->boxes = (box *)calloc(priv->l.side*priv->l.side*priv->l.n, sizeof(box));
priv->probs = (float **)calloc(priv->l.side*priv->l.side*priv->l.n, sizeof(float *));
for(int j = 0; j < priv->l.side * priv->l.side * priv->l.n; ++j)
priv->probs[j] = (float *)calloc(priv->l.classes, sizeof(float *));
return 0;
}
void Darknet::yoloImage(const QString &filename, float thresh)
{
const Mat ori = OpenCV::loadImage(getAbs(filename), -1);
Mat img;
cv::resize(ori, img, cv::Size(priv->net.w, priv->net.h));
image im = toDarkImage(img);
//image im = load_image_color((char *)qPrintable(getAbs(filename)), 0, 0);
//image sized = resize_image(im, priv->net.w, priv->net.h);
//float *X = sized.data;
float *X = im.data;
float *predictions = network_predict(priv->net, X);
float nms=.5;
const detection_layer l = priv->l;
convert_yolo_detections(predictions, l.classes, l.n, l.sqrt, l.side, 1, 1, thresh, priv->probs, priv->boxes, 0);
if (nms) do_nms_sort(priv->boxes, priv->probs, l.side*l.side*l.n, l.classes, nms);
draw_detections(im, l.side*l.side*l.n, thresh, priv->boxes, priv->probs, voc_names, 0, 20);
show_image(im, "predictions");
save_image(im, "predictions");
//show_image(sized, "resized");
free_image(im);
//free_image(sized);
}
int main()
{
int n = -6;
printf("Absoute value of %d is %u", n, getAbs(n));
getchar();
return 0;
}
void Darknet::predict(const QString &filename, float thresh)
{
const Mat ori = OpenCV::loadImage(getAbs(filename), -1);
Mat img;
cv::resize(ori, img, cv::Size(priv->net.w, priv->net.h));
image im = toDarkImage(img);
float nms = 0.5;
/* NOTE: network_predict doesn't allocate any data */
float *predictions = network_predict(priv->net, im.data);
const detection_layer l = priv->l;
convert_yolo_detections(predictions, l.classes, l.n, l.sqrt, l.side, 1, 1, thresh, priv->probs, priv->boxes, 0);
if (nms)
do_nms_sort(priv->boxes, priv->probs, l.side*l.side*l.n, l.classes, nms);
draw_detections(im, l.side*l.side*l.n, thresh, priv->boxes, priv->probs, voc_names, 0, 20);
save_image(im, (char *)qPrintable(getAbs(QString(filename).replace(".jpg", "_pred.jpg"))));
//ffDebug() << priv->net.n << priv->net.outputs << priv->l.classes;
}
int add(struct NUMBER *a,struct NUMBER *b,struct NUMBER *c)
{
int d = 0,e = 0,f_hizero = 0,i,res = 0;
struct NUMBER abs_a,abs_b;
/*絶対値を使う可能性があるので保存*/
getAbs(a,&abs_a);
getAbs(b,&abs_b);
clearByZero(c);
f_hizero = (firstNotZero(a)>firstNotZero(b))?firstNotZero(a):firstNotZero(b);
//f_hizeroは最初の非ゼロが現れる場所が高い方の非ゼロの場所を格納
/*計算部*/
if(getSign(a) == 1&&getSign(b) == 1)//a,bも+の時は通常のadd
{
for(i = 0;i <= f_hizero + 1&&i <= KETA - 1;i++)
//桁上りがある場合があるのでf_hizero + 1まで、またf_hizero == KETA-1の時はKETA-1まで
{
d = a->n[i] + b->n[i] + e;
c->n[i] = d%10;
e = d / 10;
}
if(f_hizero == KETA - 1&&e != 0)
res = -1;
}
if(getSign(a) == 1 && getSign(b) == -1)//aが+、bが-の時はa-b通常のsub
res = sub(a,&abs_b,c);
if(getSign(a) == -1 && getSign(b) == 1)//aが-、bが+の時は-a+b、つまりb-a、aとbが逆になったsub
res = sub(b,&abs_a,c);
if(getSign(a) == -1 && getSign(b) == -1)//aもbも-の時は-(a+b)、つまりaddの結果を負にする
{
res = add(&abs_a,&abs_b,c);
setSign(c,-1);//cを-にする
}
return(res);
}
/*
* g e t N o r m
*/
real_t getNorm( const real_t* const v, int_t n, int_t type )
{
int_t i;
real_t norm = 0.0;
switch ( type )
{
case 2:
for( i=0; i<n; ++i )
norm += v[i]*v[i];
return getSqrt( norm );
case 1:
for( i=0; i<n; ++i )
norm += getAbs( v[i] );
return norm;
default:
THROWERROR( RET_INVALID_ARGUMENTS );
return -INFTY;
}
}
int main( )
{
USING_NAMESPACE_QPOASES
long i;
int nWSR;
real_t tic, toc;
real_t errP=0.0, errD=0.0;
real_t *x1 = new real_t[180];
real_t *y1 = new real_t[271];
real_t *x2 = new real_t[180];
real_t *y2 = new real_t[271];
/* create sparse matrices */
SymSparseMat *H = new SymSparseMat(180, 180, H_ir, H_jc, H_val);
SparseMatrix *A = new SparseMatrix(91, 180, A_ir, A_jc, A_val);
H->createDiagInfo();
real_t* H_full = H->full();
real_t* A_full = A->full();
SymDenseMat *Hd = new SymDenseMat(180,180,180,H_full);
DenseMatrix *Ad = new DenseMatrix(91,180,180,A_full);
/* solve with dense matrices */
nWSR = 1000;
QProblem qrecipeD(180, 91);
tic = getCPUtime();
qrecipeD.init(Hd, g, Ad, lb, ub, lbA, ubA, nWSR, 0);
toc = getCPUtime();
qrecipeD.getPrimalSolution(x1);
qrecipeD.getDualSolution(y1);
fprintf(stdFile, "Solved dense problem in %d iterations, %.3f seconds.\n", nWSR, toc-tic);
/* Compute KKT tolerances */
real_t stat, feas, cmpl;
getKKTResidual( 180,91,
H_full,g,A_full,lb,ub,lbA,ubA,
x1,y1,
stat,feas,cmpl
);
printf( "stat = %e\nfeas = %e\ncmpl = %e\n\n", stat,feas,cmpl );
/* solve with sparse matrices */
nWSR = 1000;
QProblem qrecipeS(180, 91);
tic = getCPUtime();
qrecipeS.init(H, g, A, lb, ub, lbA, ubA, nWSR, 0);
toc = getCPUtime();
qrecipeS.getPrimalSolution(x2);
qrecipeS.getDualSolution(y2);
fprintf(stdFile, "Solved sparse problem in %d iterations, %.3f seconds.\n", nWSR, toc-tic);
/* Compute KKT tolerances */
real_t stat2, feas2, cmpl2;
getKKTResidual( 180,91,
H_full,g,A_full,lb,ub,lbA,ubA,
x2,y2,
stat2,feas2,cmpl2
);
printf( "stat = %e\nfeas = %e\ncmpl = %e\n\n", stat2,feas2,cmpl2 );
/* check distance of solutions */
for (i = 0; i < 180; i++)
if (getAbs(x1[i] - x2[i]) > errP)
errP = getAbs(x1[i] - x2[i]);
fprintf(stdFile, "Primal error: %9.2e\n", errP);
for (i = 0; i < 271; i++)
if (getAbs(y1[i] - y2[i]) > errD)
errD = getAbs(y1[i] - y2[i]);
fprintf(stdFile, "Dual error: %9.2e (might not be unique)\n", errD);
delete H;
delete A;
delete[] H_full;
delete[] A_full;
delete Hd;
delete Ad;
delete[] y2;
delete[] x2;
delete[] y1;
delete[] x1;
QPOASES_TEST_FOR_TRUE( stat <= 1e-15 );
QPOASES_TEST_FOR_TRUE( feas <= 1e-15 );
QPOASES_TEST_FOR_TRUE( cmpl <= 1e-15 );
QPOASES_TEST_FOR_TRUE( stat2 <= 1e-14 );
QPOASES_TEST_FOR_TRUE( feas2 <= 1e-14 );
QPOASES_TEST_FOR_TRUE( cmpl2 <= 1e-13 );
QPOASES_TEST_FOR_TRUE( errP <= 1e-13 );
return TEST_PASSED;
}
/*
* g e t M a x K K T v i o l a t i o n
*/
returnValue SolutionAnalysis::getMaxKKTviolation( QProblemB* qp, real_t& maxKKTviolation ) const
{
int i;
int nV = qp->getNV( );
real_t *tmp = new real_t[nV];
maxKKTviolation = 0.0;
/* 1) Check for Hx + g - y*A' = 0 (here: A = Id). */
for( i=0; i<nV; ++i )
tmp[i] = qp->g[i];
switch ( qp->getHessianType( ) )
{
case HST_ZERO:
/*tmp += qp->eps * qp->x[i]; */
break;
case HST_IDENTITY:
for( i=0; i<nV; ++i )
tmp[i] += qp->x[i];
break;
default:
qp->H->times(1, 1.0, qp->x, nV, 1.0, tmp, nV);
break;
}
for( i=0; i<nV; ++i )
{
tmp[i] -= qp->y[i];
if ( getAbs( tmp[i] ) > maxKKTviolation )
maxKKTviolation = getAbs( tmp[i] );
}
delete[] tmp;
/* 2) Check for lb <= x <= ub. */
for( i=0; i<nV; ++i )
{
if ( qp->lb[i] - qp->x[i] > maxKKTviolation )
maxKKTviolation = qp->lb[i] - qp->x[i];
if ( qp->x[i] - qp->ub[i] > maxKKTviolation )
maxKKTviolation = qp->x[i] - qp->ub[i];
}
/* 3) Check for correct sign of y and for complementary slackness. */
for( i=0; i<nV; ++i )
{
switch ( qp->bounds.getStatus( i ) )
{
case ST_LOWER:
if ( -qp->y[i] > maxKKTviolation )
maxKKTviolation = -qp->y[i];
if ( getAbs( ( qp->x[i] - qp->lb[i] ) * qp->y[i] ) > maxKKTviolation )
maxKKTviolation = getAbs( ( qp->x[i] - qp->lb[i] ) * qp->y[i] );
break;
case ST_UPPER:
if ( qp->y[i] > maxKKTviolation )
maxKKTviolation = qp->y[i];
if ( getAbs( ( qp->ub[i] - qp->x[i] ) * qp->y[i] ) > maxKKTviolation )
maxKKTviolation = getAbs( ( qp->ub[i] - qp->x[i] ) * qp->y[i] );
break;
default: /* inactive */
if ( getAbs( qp->y[i] ) > maxKKTviolation )
maxKKTviolation = getAbs( qp->y[i] );
break;
}
}
return SUCCESSFUL_RETURN;
}
/*
* g e t M a x K K T v i o l a t i o n
*/
returnValue SolutionAnalysis::getMaxKKTviolation( QProblem* qp, real_t& maxKKTviolation ) const
{
int i;
int nV = qp->getNV( );
int nC = qp->getNC( );
real_t *tmp = new real_t[nV];
maxKKTviolation = 0.0;
/* 1) check for Hx + g - [yFX yAC]*[Id A]' = 0. */
for( i=0; i<nV; ++i )
tmp[i] = qp->g[i];
switch ( qp->getHessianType( ) )
{
case HST_ZERO:
/*tmp += qp->eps * qp->x[i]; */
break;
case HST_IDENTITY:
for( i=0; i<nV; ++i )
tmp[i] += qp->x[i];
break;
default:
qp->H->times(1, 1.0, qp->x, nV, 1.0, tmp, nV);
break;
}
qp->A->transTimes(1, -1.0, qp->y + nV, nC, 1.0, tmp, nV);
for( i=0; i<nV; ++i )
{
tmp[i] -= qp->y[i];
if ( getAbs( tmp[i] ) > maxKKTviolation )
maxKKTviolation = getAbs( tmp[i] );
}
/* 2) Check for [lb lbA] <= [Id A]*x <= [ub ubA]. */
/* lbA <= Ax <= ubA */
real_t* Ax = new real_t[nC];
qp->A->times(1, 1.0, qp->x, nV, 0.0, Ax, nC);
for( i=0; i<nC; ++i )
{
if ( qp->lbA[i] - Ax[i] > maxKKTviolation )
maxKKTviolation = qp->lbA[i] - Ax[i];
if ( Ax[i] - qp->ubA[i] > maxKKTviolation )
maxKKTviolation = Ax[i] - qp->ubA[i];
}
/* lb <= x <= ub */
for( i=0; i<nV; ++i )
{
if ( qp->lb[i] - qp->x[i] > maxKKTviolation )
maxKKTviolation = qp->lb[i] - qp->x[i];
if ( qp->x[i] - qp->ub[i] > maxKKTviolation )
maxKKTviolation = qp->x[i] - qp->ub[i];
}
/* 3) Check for correct sign of y and for complementary slackness. */
/* bounds */
for( i=0; i<nV; ++i )
{
switch ( qp->bounds.getStatus( i ) )
{
case ST_LOWER:
if ( -qp->y[i] > maxKKTviolation )
maxKKTviolation = -qp->y[i];
if ( getAbs( ( qp->x[i] - qp->lb[i] ) * qp->y[i] ) > maxKKTviolation )
maxKKTviolation = getAbs( ( qp->x[i] - qp->lb[i] ) * qp->y[i] );
break;
case ST_UPPER:
if ( qp->y[i] > maxKKTviolation )
maxKKTviolation = qp->y[i];
if ( getAbs( ( qp->ub[i] - qp->x[i] ) * qp->y[i] ) > maxKKTviolation )
maxKKTviolation = getAbs( ( qp->ub[i] - qp->x[i] ) * qp->y[i] );
break;
default: /* inactive */
if ( getAbs( qp->y[i] ) > maxKKTviolation )
maxKKTviolation = getAbs( qp->y[i] );
break;
}
}
/* constraints */
for( i=0; i<nC; ++i )
{
switch ( qp->constraints.getStatus( i ) )
{
case ST_LOWER:
if ( -qp->y[nV+i] > maxKKTviolation )
maxKKTviolation = -qp->y[nV+i];
if ( getAbs( ( Ax[i] - qp->lbA[i] ) * qp->y[nV+i] ) > maxKKTviolation )
maxKKTviolation = getAbs( ( Ax[i] - qp->lbA[i] ) * qp->y[nV+i] );
break;
case ST_UPPER:
if ( qp->y[nV+i] > maxKKTviolation )
maxKKTviolation = qp->y[nV+i];
if ( getAbs( ( qp->ubA[i] - Ax[i] ) * qp->y[nV+i] ) > maxKKTviolation )
maxKKTviolation = getAbs( ( qp->ubA[i] - Ax[i] ) * qp->y[nV+i] );
break;
default: /* inactive */
if ( getAbs( qp->y[nV+i] ) > maxKKTviolation )
maxKKTviolation = getAbs( qp->y[nV+i] );
break;
}
}
delete[] tmp;
delete[] Ax;
return SUCCESSFUL_RETURN;
}
/*
* g e t K k t V i o l a t i o n
*/
returnValue getKktViolation( int_t nV, int_t nC,
const real_t* const H, const real_t* const g, const real_t* const A,
const real_t* const lb, const real_t* const ub, const real_t* const lbA, const real_t* const ubA,
const real_t* const x, const real_t* const y,
real_t& stat, real_t& feas, real_t& cmpl,
const real_t* const workingSetB, const real_t* const workingSetC, BooleanType hasIdentityHessian
)
{
/* Tolerance for dual variables considered zero. */
const real_t dualActiveTolerance = 1.0e3 * EPS;
int_t i, j;
real_t sum, prod;
/* Initialize residuals */
stat = feas = cmpl = 0.0;
/* check stationarity */
for (i = 0; i < nV; i++)
{
/* g term and variable bounds dual term */
if ( g != 0 )
sum = g[i] - y[i];
else
sum = 0.0 - y[i];
/* H*x term */
if ( H != 0 )
for (j = 0; j < nV; j++) sum += H[i*nV+j] * x[j];
else
{
if ( hasIdentityHessian == BT_TRUE )
sum += x[i];
}
/* A'*y term */
if ( A != 0 )
for (j = 0; j < nC; j++) sum -= A[j*nV+i] * y[nV+j];
/* update stat */
if (getAbs(sum) > stat) stat = getAbs(sum);
}
/* check primal feasibility and complementarity of bounds */
/* feasibility */
for (i = 0; i < nV; i++)
{
if ( lb != 0 )
if (lb[i] - x[i] > feas)
feas = lb[i] - x[i];
if ( ub != 0 )
if (x[i] - ub[i] > feas)
feas = x[i] - ub[i];
}
/* complementarity */
if ( workingSetB == 0 )
{
for (i = 0; i < nV; i++)
{
prod = 0.0;
/* lower bound */
if ( lb != 0 )
if (y[i] > dualActiveTolerance)
prod = (x[i] - lb[i]) * y[i];
/* upper bound */
if ( ub != 0 )
if (y[i] < -dualActiveTolerance)
prod = (x[i] - ub[i]) * y[i];
if (getAbs(prod) > cmpl) cmpl = getAbs(prod);
}
}
else
{
for (i = 0; i < nV; i++)
{
prod = 0.0;
/* lower bound */
if ( lb != 0 )
{
if ( isEqual(workingSetB[i],-1.0) == BT_TRUE )
prod = (x[i] - lb[i]) * y[i];
}
/* upper bound */
if ( ub != 0 )
{
if ( isEqual(workingSetB[i],1.0) == BT_TRUE )
prod = (x[i] - ub[i]) * y[i];
}
if (getAbs(prod) > cmpl) cmpl = getAbs(prod);
}
}
/* check primal feasibility and complementarity of constraints */
for (i = 0; i < nC; i++)
{
/* compute sum = (A*x)_i */
sum = 0.0;
if ( A != 0 )
for (j = 0; j < nV; j++)
sum += A[i*nV+j] * x[j];
/* feasibility */
if ( lbA != 0 )
if (lbA[i] - sum > feas)
feas = lbA[i] - sum;
if ( ubA != 0 )
if (sum - ubA[i] > feas)
feas = sum - ubA[i];
/* complementarity */
prod = 0.0;
/* lower bound */
if ( lbA != 0 )
{
if ( workingSetC == 0 )
{
if (y[nV+i] > dualActiveTolerance)
prod = (sum - lbA[i]) * y[nV+i];
}
else
{
if ( isEqual(workingSetC[i],-1.0) == BT_TRUE )
prod = (sum - lbA[i]) * y[nV+i];
}
}
/* upper bound */
if ( ubA != 0 )
{
if ( workingSetC == 0 )
{
if (y[nV+i] < -dualActiveTolerance)
prod = (sum - ubA[i]) * y[nV+i];
}
else
{
if ( isEqual(workingSetC[i],1.0) == BT_TRUE )
prod = (sum - ubA[i]) * y[nV+i];
}
}
if (getAbs(prod) > cmpl) cmpl = getAbs(prod);
}
return SUCCESSFUL_RETURN;
}
int main( )
{
USING_NAMESPACE_QPOASES
long i;
int_t nWSR;
real_t errP1, errP2, errP3, errD1, errD2, errD3, tic, toc;
real_t *x1 = new real_t[180];
real_t *y1 = new real_t[271];
real_t *x2 = new real_t[180];
real_t *y2 = new real_t[271];
real_t *x3 = new real_t[180];
real_t *y3 = new real_t[271];
Options options;
options.setToDefault();
options.enableEqualities = BT_TRUE;
/* create sparse matrices */
SymSparseMat *H = new SymSparseMat(180, 180, H_ir, H_jc, H_val);
SparseMatrix *A = new SparseMatrix(91, 180, A_ir, A_jc, A_val);
H->createDiagInfo();
real_t* H_full = H->full();
real_t* A_full = A->full();
SymDenseMat *Hd = new SymDenseMat(180,180,180,H_full);
DenseMatrix *Ad = new DenseMatrix(91,180,180,A_full);
/* solve with dense matrices */
nWSR = 1000;
QProblem qrecipeD(180, 91);
qrecipeD.setOptions(options);
tic = getCPUtime();
qrecipeD.init(Hd, g, Ad, lb, ub, lbA, ubA, nWSR, 0);
toc = getCPUtime();
qrecipeD.getPrimalSolution(x1);
qrecipeD.getDualSolution(y1);
fprintf(stdFile, "Solved dense problem in %d iterations, %.3f seconds.\n", (int)nWSR, toc-tic);
/* solve with sparse matrices (nullspace factorization) */
nWSR = 1000;
QProblem qrecipeS(180, 91);
qrecipeS.setOptions(options);
tic = getCPUtime();
qrecipeS.init(H, g, A, lb, ub, lbA, ubA, nWSR, 0);
toc = getCPUtime();
qrecipeS.getPrimalSolution(x2);
qrecipeS.getDualSolution(y2);
fprintf(stdFile, "Solved sparse problem in %d iterations, %.3f seconds.\n", (int)nWSR, toc-tic);
/* solve with sparse matrices (Schur complement) */
#ifndef SOLVER_NONE
nWSR = 1000;
SQProblemSchur qrecipeSchur(180, 91);
qrecipeSchur.setOptions(options);
tic = getCPUtime();
qrecipeSchur.init(H, g, A, lb, ub, lbA, ubA, nWSR, 0);
toc = getCPUtime();
qrecipeSchur.getPrimalSolution(x3);
qrecipeSchur.getDualSolution(y3);
fprintf(stdFile, "Solved sparse problem (Schur complement approach) in %d iterations, %.3f seconds.\n", (int)nWSR, toc-tic);
#endif /* SOLVER_NONE */
/* check distance of solutions */
errP1 = 0.0;
errP2 = 0.0;
errP3 = 0.0;
#ifndef SOLVER_NONE
for (i = 0; i < 180; i++)
if (getAbs(x1[i] - x2[i]) > errP1)
errP1 = getAbs(x1[i] - x2[i]);
for (i = 0; i < 180; i++)
if (getAbs(x1[i] - x3[i]) > errP2)
errP2 = getAbs(x1[i] - x3[i]);
for (i = 0; i < 180; i++)
if (getAbs(x2[i] - x3[i]) > errP3)
errP3 = getAbs(x2[i] - x3[i]);
#endif /* SOLVER_NONE */
fprintf(stdFile, "Primal error (dense and sparse): %9.2e\n", errP1);
fprintf(stdFile, "Primal error (dense and Schur): %9.2e\n", errP2);
fprintf(stdFile, "Primal error (sparse and Schur): %9.2e\n", errP3);
errD1 = 0.0;
errD2 = 0.0;
errD3 = 0.0;
for (i = 0; i < 271; i++)
if (getAbs(y1[i] - y2[i]) > errD1)
errD1 = getAbs(y1[i] - y2[i]);
#ifndef SOLVER_NONE
for (i = 0; i < 271; i++)
if (getAbs(y1[i] - y3[i]) > errD2)
errD2 = getAbs(y1[i] - y3[i]);
for (i = 0; i < 271; i++)
if (getAbs(y2[i] - y3[i]) > errD3)
errD3 = getAbs(y2[i] - y3[i]);
#endif /* SOLVER_NONE */
fprintf(stdFile, "Dual error (dense and sparse): %9.2e (might not be unique)\n", errD1);
fprintf(stdFile, "Dual error (dense and Schur): %9.2e (might not be unique)\n", errD2);
fprintf(stdFile, "Dual error (sparse and Schur): %9.2e (might not be unique)\n", errD3);
delete H;
delete A;
delete[] H_full;
delete[] A_full;
delete Hd;
delete Ad;
delete[] y3;
delete[] x3;
delete[] y2;
delete[] x2;
delete[] y1;
delete[] x1;
QPOASES_TEST_FOR_TOL( errP1,1e-13 );
QPOASES_TEST_FOR_TOL( errP2,1e-13 );
QPOASES_TEST_FOR_TOL( errP3,1e-13 );
return TEST_PASSED;
}
/** Example for calling qpOASES with sparse matrices. */
int main( )
{
long i;
int nWSR;
real_t err, tic, toc;
real_t *x1 = new real_t[180];
real_t *y1 = new real_t[271];
real_t *x2 = new real_t[180];
real_t *y2 = new real_t[271];
/* create sparse matrices */
SymSparseMat *H = new SymSparseMat(180, 180, H_ir, H_jc, H_val);
SparseMatrix *A = new SparseMatrix(91, 180, A_ir, A_jc, A_val);
H->createDiagInfo();
real_t* H_full = H->full();
real_t* A_full = A->full();
SymDenseMat *Hd = new SymDenseMat(180,180,180,H_full);
DenseMatrix *Ad = new DenseMatrix(91,180,180,A_full);
/* solve with dense matrices */
nWSR = 1000;
QProblem qrecipeD(180, 91);
tic = getCPUtime();
qrecipeD.init(Hd, g, Ad, lb, ub, lbA, ubA, nWSR, 0);
toc = getCPUtime();
qrecipeD.getPrimalSolution(x1);
qrecipeD.getDualSolution(y1);
fprintf(stdFile, "Solved dense problem in %d iterations, %.3f seconds.\n", nWSR, toc-tic);
/* solve with sparse matrices */
nWSR = 1000;
QProblem qrecipeS(180, 91);
tic = getCPUtime();
qrecipeS.init(H, g, A, lb, ub, lbA, ubA, nWSR, 0);
toc = getCPUtime();
qrecipeS.getPrimalSolution(x2);
qrecipeS.getDualSolution(y2);
fprintf(stdFile, "Solved sparse problem in %d iterations, %.3f seconds.\n", nWSR, toc-tic);
/* check distance of solutions */
err = 0.0;
for (i = 0; i < 180; i++)
if (getAbs(x1[i] - x2[i]) > err)
err = getAbs(x1[i] - x2[i]);
fprintf(stdFile, "Primal error: %9.2e\n", err);
err = 0.0;
for (i = 0; i < 271; i++)
if (getAbs(y1[i] - y2[i]) > err)
err = getAbs(y1[i] - y2[i]);
fprintf(stdFile, "Dual error: %9.2e (might not be unique)\n", err);
delete H;
delete A;
delete[] H_full;
delete[] A_full;
delete Hd;
delete Ad;
delete[] y2;
delete[] x2;
delete[] y1;
delete[] x1;
return 0;
}
int divide(struct NUMBER *a,struct NUMBER *b,struct NUMBER *c,struct NUMBER *d)
{
int k,res = 0;
struct NUMBER tmp_a,n,m,l,abs_a,abs_b;
copyNumber(a,&tmp_a);//aが破壊されないようにtmp_aにコピー
if(isZero(b) == 0)//isZeroは0か-1が返るので
return(-1);
clearByZero(c);
clearByZero(d);//c,dを0でクリア
clearByZero(&n);
clearByZero(&m);
clearByZero(&l);//作業用変数n,m,lを0でクリア
setInt(&n,1);//適当な変数に1を突っ込む
if(numComp(b,&n) == 0)//除数が1ならば
{
copyNumber(a,c);//c = a;
return(0);
}
getAbs(a,&abs_a);
getAbs(b,&abs_b);//a,bの絶対値をとる
if((getSign(a) == 1) && (getSign(b) == 1))
{
while(1)//xから何回yを引けるか調べる
{
if(numComp(&tmp_a,b) == -1)//a < bならば
break;
copyNumber(b,&n);//bを作業用変数nに代入
setInt(&m,1);//作業用変数mに1を代入
while(1)
{
copyNumber(&n,&l);
mulBy10(&l,&n);
copyNumber(&m,&l);
mulBy10(&l,&m);//mとnを10倍
if(numComp(&tmp_a,&n) == -1)//a < nならば
{
copyNumber(&n,&l);
divBy10(&l,&n);
copyNumber(&m,&l);
divBy10(&l,&m);//10倍しちゃったので10で割る。いい方法を模索中、一時的に保存する?
break;
}
}
while(1)
{
copyNumber(&tmp_a,&l);//lに現在のaを代入
sub(&l,&n,&tmp_a);//a = l -n; すなわち a -= n;
copyNumber(c,&l);//lにcを代入
add(&l,&m,c);//c = l + m;すわなち c += m;
if(numComp(&tmp_a,&n) == -1)
break;
}
}
copyNumber(&tmp_a,d);//残ったtmp_aがすなわち剰余
}
if((getSign(a) == 1) && (getSign(b) == -1))
{
res = divide(a,&abs_b,c,d);
setSign(c,-1);//+ / -は解が負であるため
}
if((getSign(a) == -1) && (getSign(b) == 1))
{
res = divide(&abs_a,b,c,d);
setSign(c,-1);//- / +は解が負であるため
setSign(d,-1);//- / +は剰余が負であるため
}
if((getSign(a) == -1)&& (getSign(b) == -1))
{
res = divide(&abs_a,&abs_b,c,d);//-x / -yは解が正であるためなにもしない
setSign(d,-1);//- / -は剰余が負であるため
}
return(res);
}
int multiple(struct NUMBER *a,struct NUMBER *b,struct NUMBER *c)
{
int e,h,res = 0,i,j,f_hizero_a,f_hizero_b;
struct NUMBER d,tmp_c,abs_a,abs_b,tmp;
clearByZero(c);
clearByZero(&tmp_c);
/*絶対値を使う場合があるので*/
getAbs(a,&abs_a);
getAbs(b,&abs_b);
if(isZero(a) == 0 || isZero(b) == 0)
return(0);//もうcは0になっているのでcBZはしない
setInt(&tmp,1);//tmpに1を代入
if(numComp(a,&tmp) == 0)//aとtmpが同じ値なら = aが1なら
{
copyNumber(b,c);//cにbをコピー
return(0);
}
if(numComp(b,&tmp) == 0)
{
copyNumber(a,c);//cにaをコピー
return(0);
}
f_hizero_a = firstNotZero(a);//aの最初の非ゼロを数える、これはjの最大値となる
f_hizero_b = firstNotZero(b);//bの最初の非ゼロを数える、これはiの最大値となる
if(getSign(a) == 1&&getSign(b) == 1)//a,bも+の時は通常のmultiple
{
for(i = 0;i <= f_hizero_b;i++)//第一ループ、求めるのはa * b
{
h = 0;
clearByZero(&d);
if(b->n[i] == 0)//a * bのbが0の時の結果は0
clearByZero(&d);
else if(b->n[i] == 1)//a * bのbが1の時の結果はa
{
for(j = 0;j <= f_hizero_a;j++)
{
d.n[j + i] = a->n[j];
}
}
else
{
for(j = 0;j <= f_hizero_a;j++)//第二ループ、求めるのはa * b_i
{
if(i + j >KETA - 1)//i + jは値を代入する部分なのでここがKETA - 1を超えていたらアウト
return(-1);
e = a->n[j] * b->n[i] + h;//eにa_j * b_iと前の繰り上がりを足す
d.n[j + i] =e % 10;//d_i+jにeの一桁目を代入
h = e / 10;//hにeの二桁目を代入 = 繰り上がり
}
}
if(i + j < KETA)
d.n[j + i] = h;//jがf_hizero_aまでなので乗算した時のhをここに代入する。
else if(h != 0)
return(-1);
res = add(c,&d,&tmp_c);
if(res == -1)//addでオーバーフローが起きた時のためのif
return(-1);
copyNumber(&tmp_c,c);
}
}
if(getSign(a) == 1 && getSign(b) == -1)//aが+、bが-の時は-(a*|b|)
{
res = multiple(a,&abs_b,c);
setSign(c,-1);
}
if(getSign(a) == -1 && getSign(b) == 1 )//aが-、bが+の時は-(|a|*b)
{
res = multiple(&abs_a,b,c);
setSign(c,-1);
}
if(getSign(a) == -1 && getSign(b) == -1)//aもbも-の時は|a| * |b|
res = multiple(&abs_b,&abs_a,c);
return(res);
}
Mat Darknet::predictFile(const QString &filename, float thresh)
{
const Mat ori = OpenCV::loadImage(getAbs(filename), -1);
return predict(ori, thresh);
}
bool isBoxIntersectingTriangle(const Box_f & box, const Triangle_f & triangle) {
/* use separating axis theorem to test overlap between triangle and box */
/* need to test for overlap in these directions: */
/* 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle */
/* we do not even need to test these) */
/* 2) normal of the triangle */
/* 3) crossproduct(edge from tri, {x,y,z}-directin) */
/* this gives 3x3=9 more tests */
Vec3 boxcenter = box.getCenter();
Vec3 boxhalfsize(0.5f * box.getExtentX(), 0.5f * box.getExtentY(), 0.5f * box.getExtentZ());
/* This is the fastest branch on Sun */
/* move everything so that the boxcenter is in (0,0,0) */
const auto v0 = triangle.getVertexA() - boxcenter;
const auto v1 = triangle.getVertexB() - boxcenter;
const auto v2 = triangle.getVertexC() - boxcenter;
/* compute triangle edges */
const auto e0 = triangle.getEdgeAB();
const auto e1 = triangle.getEdgeBC();
const auto e2 = triangle.getEdgeCA();
/* Bullet 3: */
/* test the 9 tests first (this was faster) */
float p0, p1, p2, rad, pMin, pMax;
Vec3 fe = e0.getAbs();
p0 = e0.z() * v0.y() - e0.y() * v0.z();
p2 = e0.z() * v2.y() - e0.y() * v2.z();
if (p0 < p2) {
pMin = p0;
pMax = p2;
} else {
pMin = p2;
pMax = p0;
}
rad = fe.z() * boxhalfsize.y() + fe.y() * boxhalfsize.z();
if (pMin > rad || pMax < -rad)
return 0;
p0 = -e0.z() * v0.x() + e0.x() * v0.z();
p2 = -e0.z() * v2.x() + e0.x() * v2.z();
if (p0 < p2) {
pMin = p0;
pMax = p2;
} else {
pMin = p2;
pMax = p0;
}
rad = fe.z() * boxhalfsize.x() + fe.x() * boxhalfsize.z();
if (pMin > rad || pMax < -rad)
return 0;
p1 = e0.y() * v1.x() - e0.x() * v1.y();
p2 = e0.y() * v2.x() - e0.x() * v2.y();
if (p2 < p1) {
pMin = p2;
pMax = p1;
} else {
pMin = p1;
pMax = p2;
}
rad = fe.y() * boxhalfsize.x() + fe.x() * boxhalfsize.y();
if (pMin > rad || pMax < -rad)
return 0;
fe = e1.getAbs();
p0 = e1.z() * v0.y() - e1.y() * v0.z();
p2 = e1.z() * v2.y() - e1.y() * v2.z();
if (p0 < p2) {
pMin = p0;
pMax = p2;
} else {
pMin = p2;
pMax = p0;
}
rad = fe.z() * boxhalfsize.y() + fe.y() * boxhalfsize.z();
if (pMin > rad || pMax < -rad)
return 0;
p0 = -e1.z() * v0.x() + e1.x() * v0.z();
p2 = -e1.z() * v2.x() + e1.x() * v2.z();
if (p0 < p2) {
pMin = p0;
pMax = p2;
} else {
pMin = p2;
pMax = p0;
}
rad = fe.z() * boxhalfsize.x() + fe.x() * boxhalfsize.z();
if (pMin > rad || pMax < -rad)
return 0;
p0 = e1.y() * v0.x() - e1.x() * v0.y();
p1 = e1.y() * v1.x() - e1.x() * v1.y();
if (p0 < p1) {
pMin = p0;
pMax = p1;
} else {
pMin = p1;
pMax = p0;
}
rad = fe.y() * boxhalfsize.x() + fe.x() * boxhalfsize.y();
if (pMin > rad || pMax < -rad)
return 0;
fe = e2.getAbs();
p0 = e2.z() * v0.y() - e2.y() * v0.z();
p1 = e2.z() * v1.y() - e2.y() * v1.z();
if (p0 < p1) {
pMin = p0;
pMax = p1;
} else {
pMin = p1;
pMax = p0;
}
rad = fe.z() * boxhalfsize.y() + fe.y() * boxhalfsize.z();
if (pMin > rad || pMax < -rad)
return 0;
p0 = -e2.z() * v0.x() + e2.x() * v0.z();
p1 = -e2.z() * v1.x() + e2.x() * v1.z();
if (p0 < p1) {
pMin = p0;
pMax = p1;
} else {
pMin = p1;
pMax = p0;
}
rad = fe.z() * boxhalfsize.x() + fe.x() * boxhalfsize.z();
if (pMin > rad || pMax < -rad)
return 0;
p1 = e2.y() * v1.x() - e2.x() * v1.y();
p2 = e2.y() * v2.x() - e2.x() * v2.y();
if (p2 < p1) {
pMin = p2;
pMax = p1;
} else {
pMin = p1;
pMax = p2;
}
rad = fe.y() * boxhalfsize.x() + fe.x() * boxhalfsize.y();
if (pMin > rad || pMax < -rad)
return 0;
/* Bullet 1: */
/* first test overlap in the {x,y,z}-directions */
/* find min, max of the triangle each direction, and test for overlap in */
/* that direction -- this is equivalent to testing a minimal AABB around */
/* the triangle against the AABB */
/* test in X-direction */
if (std::min(std::min(v0.x(), v1.x()), v2.x()) > boxhalfsize[0]
|| std::max(std::max(v0.x(), v1.x()), v2.x()) < -boxhalfsize[0])
return 0;
/* test in Y-direction */
if (std::min(std::min(v0.y(), v1.y()), v2.y()) > boxhalfsize[1]
|| std::max(std::max(v0.y(), v1.y()), v2.y()) < -boxhalfsize[1])
return 0;
/* test in Z-direction */
if (std::min(std::min(v0.z(), v1.z()), v2.z()) > boxhalfsize[2]
|| std::max(std::max(v0.z(), v1.z()), v2.z()) < -boxhalfsize[2])
return 0;
/* Bullet 2: */
/* test if the box intersects the plane of the triangle */
/* compute plane equation of triangle: normal*x+d=0 */
Vec3 normal = e0.cross(e1);
float d = -normal.dot(v0); /* plane eq: normal.x+d=0 */
Vec3 vmin, vmax;
for (int q = 0; q < 3; q++) {
if (normal[q] > 0.0f) {
vmin[q] = -boxhalfsize[q];
vmax[q] = boxhalfsize[q];
} else {
vmin[q] = boxhalfsize[q];
vmax[q] = -boxhalfsize[q];
}
}
if (normal.dot(vmin) + d > 0.0f)
return 0;
if (normal.dot(vmax) + d >= 0.0f)
return 1;
return 0;
}
