arma::cx_mat PZStability::oo_rotation(const arma::vec & x, bool spin) const {
if(x.n_elem != count_params()) {
ERROR_INFO();
throw std::runtime_error("Inconsistent parameter size.\n");
}
if(spin && restr) {
ERROR_INFO();
throw std::runtime_error("Incompatible arguments.\n");
}
// Amount of occupied orbitals
size_t o=oa;
if(spin)
o=ob;
// Calculate offset
size_t ioff0=0;
// Canonical rotations
if(cancheck) {
ioff0=count_ov_params(oa,va);
if(!restr)
ioff0+=count_ov_params(ob,vb);
}
// Occupied rotations
if(spin)
ioff0+=count_oo_params(oa);
// Collect real part of rotation
arma::mat kappa(o,o);
kappa.zeros();
for(size_t i=0;i<o;i++)
for(size_t j=0;j<i;j++) {
size_t idx=i*(i-1)/2 + j + ioff0;
kappa(i,j)=x(idx);
kappa(j,i)=-x(idx);
}
// Rotation matrix
arma::cx_mat R;
if(!cplx)
R=kappa*COMPLEX1;
else {
// Imaginary part of rotation.
arma::mat lambda(o,o);
lambda.zeros();
// Offset
size_t ioff=o*(o-1)/2 + ioff0;
for(size_t i=0;i<o;i++)
for(size_t j=0;j<=i;j++) {
size_t idx=ioff + i*(i+1)/2 + j;
lambda(i,j)=x(idx);
lambda(j,i)=-x(idx);
}
// Matrix
R=kappa*COMPLEX1 + lambda*COMPLEXI;
}
// R is anti-hermitian. Get its eigenvalues and eigenvectors
arma::cx_mat Rvec;
arma::vec Rval;
bool diagok=arma::eig_sym(Rval,Rvec,-COMPLEXI*R);
if(!diagok) {
arma::mat Rt;
Rt=arma::real(R);
Rt.save("R_re.dat",arma::raw_ascii);
Rt=arma::imag(R);
Rt.save("R_im.dat",arma::raw_ascii);
ERROR_INFO();
throw std::runtime_error("Unitary optimization: error diagonalizing R.\n");
}
// Rotation is
arma::cx_mat Roo(Rvec*arma::diagmat(arma::exp(COMPLEXI*Rval))*arma::trans(Rvec));
// Check unitarity
arma::cx_mat prod=arma::trans(Roo)*Roo-arma::eye(Roo.n_cols,Roo.n_cols);
double norm=rms_cnorm(prod);
if(norm>=sqrt(DBL_EPSILON))
throw std::runtime_error("Matrix is not unitary!\n");
return Roo;
}
/*************************************************************************
Tests EVD problem
*************************************************************************/
static void testevdproblem(const ap::real_1d_array& d,
const ap::real_1d_array& e,
int n,
double& valerr,
double& vecerr,
bool& wnsorted,
int& failc)
{
ap::real_1d_array lambda;
ap::real_1d_array lambdaref;
ap::real_2d_array z;
ap::real_2d_array zref;
ap::real_2d_array a1;
ap::real_2d_array a2;
ap::real_2d_array ar;
bool wsucc;
int i;
int j;
int k;
int m;
int i1;
int i2;
double v;
double a;
double b;
lambdaref.setbounds(0, n-1);
zref.setbounds(0, n-1, 0, n-1);
a1.setbounds(0, n-1, 0, n-1);
a2.setbounds(0, n-1, 0, n-1);
//
// Reference EVD
//
if( !refevd(d, e, n, lambdaref, zref) )
{
failc = failc+1;
return;
}
//
// Test different combinations
//
for(i1 = 0; i1 <= n-1; i1++)
{
for(i2 = i1; i2 <= n-1; i2++)
{
//
// Select A, B
//
if( i1>0 )
{
a = 0.5*(lambdaref(i1)+lambdaref(i1-1));
}
else
{
a = lambdaref(0)-1;
}
if( i2<n-1 )
{
b = 0.5*(lambdaref(i2)+lambdaref(i2+1));
}
else
{
b = lambdaref(n-1)+1;
}
//
// Test interval, no vectors
//
lambda.setbounds(0, n-1);
for(i = 0; i <= n-1; i++)
{
lambda(i) = d(i);
}
if( !smatrixtdevdr(lambda, e, n, 0, a, b, m, z) )
{
failc = failc+1;
return;
}
if( m!=i2-i1+1 )
{
failc = failc+1;
return;
}
for(k = 0; k <= m-1; k++)
{
valerr = ap::maxreal(valerr, fabs(lambda(k)-lambdaref(i1+k)));
}
//
// Test indexes, no vectors
//
lambda.setbounds(0, n-1);
for(i = 0; i <= n-1; i++)
{
lambda(i) = d(i);
}
if( !smatrixtdevdi(lambda, e, n, 0, i1, i2, z) )
{
failc = failc+1;
return;
}
m = i2-i1+1;
for(k = 0; k <= m-1; k++)
{
valerr = ap::maxreal(valerr, fabs(lambda(k)-lambdaref(i1+k)));
}
//
// Test interval, transform vectors
//
lambda.setbounds(0, n-1);
for(i = 0; i <= n-1; i++)
{
lambda(i) = d(i);
}
a1.setbounds(0, n-1, 0, n-1);
a2.setbounds(0, n-1, 0, n-1);
for(i = 0; i <= n-1; i++)
{
for(j = 0; j <= n-1; j++)
{
a1(i,j) = 2*ap::randomreal()-1;
a2(i,j) = a1(i,j);
}
}
if( !smatrixtdevdr(lambda, e, n, 1, a, b, m, a1) )
{
failc = failc+1;
return;
}
if( m!=i2-i1+1 )
{
failc = failc+1;
return;
}
for(k = 0; k <= m-1; k++)
{
valerr = ap::maxreal(valerr, fabs(lambda(k)-lambdaref(i1+k)));
}
ar.setbounds(0, n-1, 0, m-1);
for(i = 0; i <= n-1; i++)
{
for(j = 0; j <= m-1; j++)
{
v = ap::vdotproduct(a2.getrow(i, 0, n-1), zref.getcolumn(i1+j, 0, n-1));
ar(i,j) = v;
}
}
for(j = 0; j <= m-1; j++)
{
v = ap::vdotproduct(a1.getcolumn(j, 0, n-1), ar.getcolumn(j, 0, n-1));
if( v<0 )
{
ap::vmul(ar.getcolumn(j, 0, n-1), -1);
}
}
for(i = 0; i <= n-1; i++)
{
for(j = 0; j <= m-1; j++)
{
vecerr = ap::maxreal(vecerr, fabs(a1(i,j)-ar(i,j)));
}
}
//
// Test indexes, transform vectors
//
lambda.setbounds(0, n-1);
for(i = 0; i <= n-1; i++)
{
lambda(i) = d(i);
}
a1.setbounds(0, n-1, 0, n-1);
a2.setbounds(0, n-1, 0, n-1);
for(i = 0; i <= n-1; i++)
{
for(j = 0; j <= n-1; j++)
{
a1(i,j) = 2*ap::randomreal()-1;
a2(i,j) = a1(i,j);
}
}
if( !smatrixtdevdi(lambda, e, n, 1, i1, i2, a1) )
{
failc = failc+1;
return;
}
m = i2-i1+1;
for(k = 0; k <= m-1; k++)
{
valerr = ap::maxreal(valerr, fabs(lambda(k)-lambdaref(i1+k)));
}
ar.setbounds(0, n-1, 0, m-1);
for(i = 0; i <= n-1; i++)
{
for(j = 0; j <= m-1; j++)
{
v = ap::vdotproduct(a2.getrow(i, 0, n-1), zref.getcolumn(i1+j, 0, n-1));
ar(i,j) = v;
}
}
for(j = 0; j <= m-1; j++)
{
v = ap::vdotproduct(a1.getcolumn(j, 0, n-1), ar.getcolumn(j, 0, n-1));
if( v<0 )
{
ap::vmul(ar.getcolumn(j, 0, n-1), -1);
}
}
for(i = 0; i <= n-1; i++)
{
for(j = 0; j <= m-1; j++)
{
vecerr = ap::maxreal(vecerr, fabs(a1(i,j)-ar(i,j)));
}
}
//
// Test interval, do not transform vectors
//
lambda.setbounds(0, n-1);
for(i = 0; i <= n-1; i++)
{
lambda(i) = d(i);
}
z.setbounds(0, 0, 0, 0);
if( !smatrixtdevdr(lambda, e, n, 2, a, b, m, z) )
{
failc = failc+1;
return;
}
if( m!=i2-i1+1 )
{
failc = failc+1;
return;
}
for(k = 0; k <= m-1; k++)
{
valerr = ap::maxreal(valerr, fabs(lambda(k)-lambdaref(i1+k)));
}
for(j = 0; j <= m-1; j++)
{
v = ap::vdotproduct(z.getcolumn(j, 0, n-1), zref.getcolumn(i1+j, 0, n-1));
if( v<0 )
{
ap::vmul(z.getcolumn(j, 0, n-1), -1);
}
}
for(i = 0; i <= n-1; i++)
{
for(j = 0; j <= m-1; j++)
{
vecerr = ap::maxreal(vecerr, fabs(z(i,j)-zref(i,i1+j)));
}
}
//
// Test interval, do not transform vectors
//
lambda.setbounds(0, n-1);
for(i = 0; i <= n-1; i++)
{
lambda(i) = d(i);
}
z.setbounds(0, 0, 0, 0);
if( !smatrixtdevdi(lambda, e, n, 2, i1, i2, z) )
{
failc = failc+1;
return;
}
m = i2-i1+1;
for(k = 0; k <= m-1; k++)
{
valerr = ap::maxreal(valerr, fabs(lambda(k)-lambdaref(i1+k)));
}
for(j = 0; j <= m-1; j++)
{
v = ap::vdotproduct(z.getcolumn(j, 0, n-1), zref.getcolumn(i1+j, 0, n-1));
if( v<0 )
{
ap::vmul(z.getcolumn(j, 0, n-1), -1);
}
}
for(i = 0; i <= n-1; i++)
{
for(j = 0; j <= m-1; j++)
{
vecerr = ap::maxreal(vecerr, fabs(z(i,j)-zref(i,i1+j)));
}
}
}
}
}
void Foam::calc(const argList& args, const Time& runTime, const fvMesh& mesh)
{
bool writeResults = !args.optionFound("noWrite");
IOobject Theader
(
"T",
runTime.timeName(),
mesh,
IOobject::MUST_READ
);
IOobject qheader
(
"q",
runTime.timeName(),
mesh,
IOobject::MUST_READ
);
IOdictionary transportProperties
(
IOobject
(
"transportProperties",
runTime.constant(),
mesh,
IOobject::MUST_READ,
IOobject::NO_WRITE
)
);
dimensionedScalar Cpa
(
transportProperties.lookup("Cpa")
);
dimensionedScalar Cpv
(
transportProperties.lookup("Cpv")
);
dimensionedScalar lambda
(
transportProperties.lookup("lambda")
);
// Fluid density
dimensionedScalar rho
(
transportProperties.lookup("rho")
);
dimensionedScalar TRef
(
transportProperties.lookup("TRef")
);
dimensionedScalar qRef
(
transportProperties.lookup("qRef")
);
if (qheader.headerOk() && Theader.headerOk())
{
Info<< " Reading q" << endl;
volScalarField q(qheader, mesh);
Info<< " Reading T" << endl;
volScalarField T(Theader, mesh);
// specific enthalpy of dry air hda - ASHRAE 1.8
volScalarField hda("hda", rho*Cpa*T-rho*Cpa*TRef);
// specific enthalpy of dry vapor
volScalarField hdv("hdv", rho*q*(lambda + Cpv*T) - rho*qRef*(lambda + Cpv*TRef));
// specific enthalpy for moist air hmoist - ASHRAE 1.8
volScalarField hmoist("hmoist", hda + hdv);
if (writeResults)
{
hda.write();
hdv.write();
hmoist.write();
}
else
{
Info<< " Min hda : " << min(hda).value() << " [J/m3]"
<< "\n Max hda : "<< max(hda).value() << " [J/m3]" << endl;
Info<< " Min hdv : " << min(hdv).value() << " [J/m3]"
<< "\n Max hdv : "<< max(hdv).value() << " [J/m3]" << endl;
Info<< " Min hmoist : " << min(hmoist).value() << " [J/m3]"
<< "\n Max hmoist : "<< max(hmoist).value() << " [J/m3]" << endl;
}
// print results
//
// surfaceScalarField hdaBoundary =
// fvc::interpolate(hda);
//
// const surfaceScalarField::GeometricBoundaryField& patchhda =
// hdaBoundary.boundaryField();
// // const surfaceScalarField::GeometricBoundaryField& patchCondHeatFlux =
// // condHeatFluxNormal.boundaryField();
// // const surfaceScalarField::GeometricBoundaryField& patchTurbHeatFlux =
// // turbHeatFluxNormal.boundaryField();
// // const surfaceScalarField::GeometricBoundaryField& patchTotHeatFlux =
// // totHeatFluxNormal.boundaryField();
//
// Info<< "\nHeat at the boundaries " << endl;
// forAll(patchhda, patchi)
// {
// if ( (!isA<emptyFvPatch>(mesh.boundary()[patchi])) &&
// (mesh.boundary()[patchi].size() > 0) )
// {
// Info<< " "
// << mesh.boundary()[patchi].name()
// << "\n Total area [m2] : "
// << gSum(mesh.magSf().boundaryField()[patchi])
// << "\n Integral Energy [J] : "
// << gSum
// (
// mesh.magSf().boundaryField()[patchi]
// *patchhda[patchi]
// )
// << nl << endl;
// }
// }
// Info << endl;
}
else
{
Info<< " No q or No T" << endl;
}
Info<< "\nEnd\n" << endl;
}
double CosinePoissonProcess::event_rate(const DateTime &t)const{
double dt = t - origin_;
return lambda() * (1 + std::cos(frequency() * dt));
}
DEM typ1 (DEM x)
{
int o, o1, o2;
DEM a, b, c, c1, f, x1, y, z, v, PIab, bz, s, tf, tf1, tz, tz1, t, n, n1, r;
#if 1
return U(Order(0));
#else
o = order(x);
o1 = suc_order(o);
o2 = suc_order(o1);
switch (node(x))
{
case _Var : return subdem(0,x);
case _It:
a = subdem(0,x);
t = Fott (o1, a, a);
return t;
case _IT :
a = Var ("a", U(o1));
return PIott (o2, U(o1), lambda (a,
Fott (/*suc_order*/(o1), a, a) ));
case _Kt:
a = subdem(0,x);
b = subdem(1,x);
t = Fott (o1, a, Fott (o1, b, a));
return t;
case _KT :
a = Var ("a", U(o1));
b = Var ("b", U(o1));
return PIott (o2, U(o1), lambda (a,
PIott (o2, U(o1), lambda(b,
Fott (o1, a, Fott (o1, b, a)) )) ));
case _St:
a = subdem(0,x);
b = subdem(1,x);
c = subdem(2,x);
z = Var ("z", a);
y = Var ("y", PIott (o1, a, b));
t = Fott (o1, PIott (o1, a, lambda (z,
PIott(o1, ap(b,z),ap(c,z)))),
PIott (o1, PIott (o1, a, b), lambda (y,
Fott (o1, a, ap(ap(c,z),ap(y,z)))) ));
return t;
case _ST :
a = Var ("a", U(o1));
b = Var ("b", Fott (o2, a, U(o1)));
z = Var ("z", a);
c = Var ("c", PIott (o1, a, lambda (z, Fott (o2, ap(b,z), U(o1)))));
y = Var ("y", PIott (o1, a, b));
return PIott (o2, U(o1), lambda (a,
Fott (o2, Fott (o2, a, U(o1)), lambda (b,
PIott (o1, PIott (o1, a, lambda (z,
Fott (o2, ap(b,z), U(o1)))), lambda (c,
Fott (o1, PIott (o1, a, lambda (z,
PIott(o1, ap(b,z),ap(c,z)))),
PIott (o1, PIott (o1, a, b), lambda (y,
Fott (o1, a, ap(ap(c,z),ap(y,z)))) )))
) ))));
case _Yt:
a = subdem(0,x);
t = Fott (o1, Fott (o1, a, a), a);
return t;
case _Ord:
return U(Order(1));
case _Zero:
return Ord;
case _Suc:
return Fott (Order(1), Ord, Ord);
case _Lim:
return Fott (Order(1), Fott (Order(1), Ord, Ord), Ord);
case _Rept:
o1 = Order(1);
a = subdem(0,x);
t = Fott (o1, Ord,
Fott (o1, Fott (o1, a, a), Fott (o1, a, a)));
return t;
case _Testt:
o1 = Order(1);
a = subdem(0,x);
n = Var ("n", Ord);
n1 = Var ("n1", Ord);
t = PIott (o1, Ord, lambda (n1, Fott (o1, ap (a, Zero),
Ftt ( PIott (o1, Ord, lambda (n, ap (a, ap (Suc, n)))),
ap (a, n1) ) )));
return t;
case _F:
return Fott (o1, U(o), Fott (o1, U(o), U(o)));
case _PI:
a = Var ("a", U(o));
return PIott (o1, U(o), lambda (a,
Fott (o1, Fott (o1, a, U(o)), U(o)) ));
case _U: return U (suc_order (order_u (x)));
case _ap:
if (depth(x) == 1 && node(s=rfnc(1,x)) == _Kt)
{
a = subdem(0,s);
b = subdem(1,s);
o = order(s);
return Fott (o, b, a);
}
if (node(subdem(0,x)) == _ap &&
node(subdem(0,subdem(0,x))) == _ap &&
node(subdem(0,subdem(0,subdem(0,x)))) == _KT)
{
a = subdem(1,subdem(0,subdem(0,x)));
b = subdem(1,subdem(0,x));
o = order(a);
return Fott (o, b, a);
}
if (depth (x) == 2 && node(s=rfnc(2,x)) == _St)
{
a = subdem(0,s);
b = subdem(1,s);
c = subdem(2,s);
goto Sabc;
}
if (depth (x) == 5 && node(s=rfnc(5,x)) == _ST)
{
/* S a:Uo+1 b:(Uo(a)+1) c:(a)>\z:a.((bz)>Uo+1)
x:(a)>\z:a.(cz(bz)) y:(a)>b z:a = xz(yz):cz(yz)
xz:(bz)>cz, yz:bz
S a b c x y : a ->> \z:a. cz(yz)
*/
a = subdem(1,rfnc(4,x));
b = subdem(1,rfnc(3,x));
c = subdem(1,rfnc(2,x));
Sabc:
x1 = subdem(1,rfnc(1,x));
y = subdem(1,rfnc(0,x));
z = Var ("z", a);
o = order(s);
o1 = suc_order (o);
/* t = PIott (o1, a, lambda (z, ap (ap (c, z), ap (y, z)))); */
v = Var ("v", simplif (ap (b, z)));
c1 = lambda (z, lambda (v, U(o1)));
trace_dem ("c = ", c);
t = PIott (o1, a, Sotttxx (o, a, b, c1, c, y));
return t;
}
if (node(subdem(0,x)) == _ap &&
node(subdem(0,subdem(0,x))) == _F)
{
/*
if (typ (subdem(1,subdem(0,x))) == typ (subdem(1,x)))
return typ (subdem(1,x));
error ("F applied to different types", __LINE__, x);
*/
return unio (typ(subdem(1,subdem(0,x))),
typ(subdem(1,x)));
}
f = fnc (x);
z = arg (x);
/* PIab = typ (f);
b = arg (PIab); */
tf = typ (f);
tz = typ (z);
new_tz:
print_string (print_param, "typ(z) = ");
print_dem (print_param, tz);
new_tf:
print_string (print_param, " subdem(1,subdem(0,tf)) = ");
print_dem (print_param, subdem(1,subdem(0,tf)));
print_string (print_param, "\n");
if (node(tf) == _ap &&
node(subdem(0,tf)) == _ap &&
node(subdem(0,subdem(0,tf))) == _F &&
inclus (tz, subdem(1,subdem(0,tf))))
return subdem(1,tf);
if (node(tf) == _ap &&
node(subdem(0,tf)) == _ap &&
node(subdem(0,subdem(0,tf))) == _PI &&
inclus (tz, subdem(1,subdem(0,tf))))
{
/* return ap (subdem(1,tf), z); */
tf1 = subdem(1,tf);
print_string (print_param, "tf1 = ");
print_dem (print_param, tf1);
print_string (print_param, "\nz = ");
print_dem (print_param, z);
r = ap (tf1, z);
print_string (print_param, "\nr = ");
print_dem (print_param, r);
print_string (print_param, "\n");
return r;
}
print_string (print_param, "Bad type of function ");
print_dem (print_param, f);
print_string (print_param, " of type ");
print_dem (print_param, tf);
print_string (print_param, " applied to ");
print_dem (print_param, z);
print_string (print_param, " of type ");
print_dem (print_param, tz);
print_string (print_param, "\n");
tf1 = simplif (tf);
if (tf1 != tf)
{
tf = tf1;
goto new_tf;
}
tz1 = simplif (tz);
if (tz1 != tz)
{
tz = tz1;
goto new_tz;
}
print_string (print_param, "Bad type of function ");
print_dem (print_param, f);
print_string (print_param, " of type ");
print_dem (print_param, tf);
print_string (print_param, " applied to ");
print_dem (print_param, z);
print_string (print_param, " of type ");
print_dem (print_param, tz);
print_string (print_param, "\n");
error ("Bad type of function : ", __LINE__, tf);
/*bz = ap (b, z);
return bz;*/
case _Lambda:
z = subdem(0,x);
r = subdem(1,x);
a = typ (z);
bz = typ (r);
b = lambda (z, bz);
t = PItt (a, b);
return t;
break;
default:
print_string (print_param, "typ not implemented for ");
print_dem (print_param, x);
print_string (print_param, "\n");
return U(-1);
}
#endif
}
int main( int argc, char ** argv )
{
std::map<std::string, std::string> options;
for (int i = 1; i < argc; i++)
{
std::string opt = argv[i];
std::string delimiter = "=";
size_t del_pos = opt.find(delimiter);
if (del_pos == std::string::npos || del_pos+1 == opt.size())
{
std::cerr << "Ill posed command line argument: " << argv[i] << std::endl;
return 64;
}
std::string key = opt.substr(0, opt.find(delimiter));
std::string value = opt.substr(opt.find(delimiter)+1, opt.size());
options[key] = value;
std::cout << "Registering option " << key << " : " << value << std::endl;
}
/* Set the random seed for the gsl random generator */
int seed;
if (options.count(OPT_SEED) == 1)
{
seed = atoi ( options[OPT_SEED].c_str() );
}
else
{
seed = (int) time(0);
}
/* Set the initial policy file */
std::string start_policy;
if (options.count(OPT_START_POL_FILE) == 1)
{
start_policy = MDPTETRIS_DATA_PATH(options[OPT_START_POL_FILE]);
}
else
{
start_policy = MDPTETRIS_DATA_PATH("starting_policy01.dat");
}
/* Set the initial policy file */
std::string piece_file;
if (options.count(OPT_PIECE_FILE) == 1)
{
piece_file = MDPTETRIS_DATA_PATH(options[OPT_PIECE_FILE]);
}
else
{
piece_file = MDPTETRIS_DATA_PATH("pieces4.dat");
}
/* Optionally set the initial Sigma
* For Cross entropy, this sets all the variance
* intries to the value given.
* In CMA this is the initial step-size.
* */
ExperimentOptionType<double> initialSigma(false, 1.0);
if (options.count(OPT_INITIAL_SIGMA) == 1)
{
double s = atof ( options[OPT_INITIAL_SIGMA].c_str() );
initialSigma = ExperimentOptionType<double>(true, s);
}
/* CMA specific lower bound.
* The will force the CMA step-size to keep
* the smallest eignvalue of the covariance matrix
* multiplied by the step-size to stay above this
* value.
* */
ExperimentOptionType<double> lowerBound(false, 1E-20);
if (options.count(OPT_LOWER_BOUND) == 1)
{
double s = atof ( options[OPT_LOWER_BOUND].c_str() );
lowerBound = ExperimentOptionType<double>(true, s);
}
/* Register option for population size */
ExperimentOptionType<unsigned int> lambda(false, 100);
if (options.count(OPT_LAMBDA) == 1)
{
int i = atoi( options[OPT_LAMBDA].c_str() );
lambda = ExperimentOptionType<unsigned int>(true, i);
}
/* Register option for offspriong size */
ExperimentOptionType<unsigned int> offspring(false, 100);
if (options.count(OPT_OFFSPRING) == 1)
{
int i = atoi( options[OPT_OFFSPRING].c_str() );
offspring = ExperimentOptionType<unsigned int>(true, i);
}
unsigned int nbGames = 1;
if (options.count(OPT_NB_GAMES) == 1)
{
nbGames = atoi ( options[OPT_NB_GAMES].c_str() );
}
unsigned int nbLearnGames = 30;
if (options.count(OPT_NB_LEARNING_GAMES) == 1)
{
nbLearnGames = atoi ( options[OPT_NB_LEARNING_GAMES].c_str() );
}
shark::CrossEntropy::SamplingNoise noiseType = shark::CrossEntropy::NONE;
if (options.count(OPT_NOISETYPE) == 1)
{
switch (atoi( options[OPT_NOISETYPE].c_str() ))
{
case 0:
{
noiseType = shark::CrossEntropy::NONE;
break;
}
case 1:
{
noiseType = shark::CrossEntropy::CONSTANT;
break;
}
case 2:
{
noiseType = shark::CrossEntropy::LINEAR_DECREASING;
break;
}
default:
{
break;
}
}
}
/* Cross Entropy specific for noise type */
double noise = 0;
if (options.count(OPT_NOISE) == 1)
{
noise = atof( options[OPT_NOISE].c_str() );
}
unsigned int boardWidth = 10;
unsigned int boardHeight = 20;
StoppingCriteria stoppingCriteria = STOP_BY_ITERATION;
unsigned int maxIterations = 80; /* Default stop at 80 iterations */
unsigned int maxAgents = 80000; /* Default agents to evaluate is 80000 */
if (options.count(OPT_MAXITER) == 1)
{
maxIterations = atoi ( options[OPT_MAXITER].c_str() );
stoppingCriteria = STOP_BY_ITERATION;
}
else if (options.count(OPT_MAX_AGENTS) == 1)
{
maxAgents = atoi ( options[OPT_MAX_AGENTS].c_str() );
stoppingCriteria = STOP_BY_AGENTS_EVALUATED;
}
std::string outputfile = std::string("");
if (options.count(OPT_OUTPUTNAME) == 1)
{
outputfile = std::string(options[OPT_OUTPUTNAME]) + ".txt";
}
if ( options.count(OPT_OPTIMIZER) == 1 )
{
if ( options[OPT_OPTIMIZER].compare("cma") == 0 )
{
useCMA(
start_policy,
piece_file,
nbGames,
nbLearnGames,
boardWidth,
boardHeight,
seed,
initialSigma,
maxIterations,
maxAgents,
stoppingCriteria,
std::cout,
outputfile,
lowerBound,
lambda,
offspring
);
}
else if ( options[OPT_OPTIMIZER].compare("ce") == 0 )
{
useCE(
start_policy,
piece_file,
nbGames,
nbLearnGames,
boardWidth,
boardHeight,
seed,
initialSigma,
maxIterations,
maxAgents,
stoppingCriteria,
std::cout,
outputfile,
noiseType,
noise,
lambda,
offspring
);
}
}
else
{
std::cerr << "Optimizer not recognized!" << std::endl;
return 64;
}
}
double alpha(Vars UL, Vars UR, double pm){
double alph = MAX(0., pm*lambda(UR, pm), pm*lambda(UL,pm));
//printf("%f %f %f %f\n", UL.press, UL.mass, UR.press, UR.mass);
return(alph);
}
int main(int argc, char **argv)
{
cout << "OneXOneYTwenty" << endl;
srand(time(NULL));
DContainer *zX = new DContainer(20,1); // alphabet
for(int i=0;i<20;i++){
(*zX) <<i;
}
vector<double> lambda(3);
lambda[0] = 0;
lambda[1] = 1;
lambda[2] = 5;
vector<vector<int > > alphX(1,vector<int>(0));
alphX[0].push_back(0);
vector<vector<int > > alphY(1,vector<int>(0));
alphY[0].push_back(0);
Test *testval = new Test(1,1,100,lambda, *zX, *zX, alphX, alphY); //get data
for(int i=0;i<20; i++){
for(int j=0; j<20; j++){
cout << i << " " << j << endl;
cout << "exakt: " << testval->getProp(i,j) << endl;
}
}
/*
DContainer *zX = new DContainer(2,1); // alphabet
*zX << 0 << 1;
DContainer *zY = new DContainer(2,1); // alphabet
*zY << 0 << 1;
vector<double> lambda(3);
lambda[0] = 0;
lambda[1] = 1;
lambda[2] = 5;
vector<vector<int > > alphbX(4,vector<int>(0));
alphbX[0].push_back(0);
alphbX[0].push_back(1);
vector<vector<int > > alphbY(4,vector<int>(0));
alphbY[0].push_back(0);
IsParameter param;
param.lambdavalue = 1.0;
param.lambdadeltaval = 1.0;
param.sigma = 0.01; //TODO find best sigma value
param.maxit = 10;
param.konv = 0.000001;
param.time = true;
param.test = true;
param.seconds = 100;
vector<int> cases(2);
cases[0]=0;
cases[1]=1;
Test *test = new Test(2,1,10000,lambda, *zX,*zY,alphbX, alphbY);
test->compareCases(param,cases);
//(DContainer &eX, DContainer &eY, DContainer &aX, DContainer &aY, IsParameter param, int version)
TestMI *testmi = new TestMI(test->getvalX(),test->getvalY(),*zX,*zY,param,0);
cout << testmi->getMI() << endl;
TestMI *testmisc = new TestMI(test->getvalX(),test->getvalY(),*zX,*zY,param,1);
cout << testmisc->getMI() << endl;
*/
SparseMatrix *m= new SparseMatrix(-3);
cout << (*m)(1,1,1) << endl;
(*m)(1,1,1)= 5;
cout << (*m)(1,1,1) << endl;
cout << " hier " << endl;
}
int main(int argc, char *argv[])
{
#include "setRootCase.H"
#include "createTime.H"
#include "createMesh.H"
#include "createFields.H"
// simpleControl simple(mesh);
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
Info<< "\nCalculating scalar transport\n" << endl;
// #include "CourantNo.H"
//vector lambda(0,0,0); //createfields
vectorField lambda(1, vector(0,0,0));
//Info<< "lambda " << lambda << endl;
scalar theta = 0.6; //0.6;
//label patchID_to = mesh.boundaryMesh().findPatchID("to");
//const fvPatchVectorField fvp = U.boundaryField()[patchID_to];
//fileName fvp(runTime.path()/runTime.timeName());
//OFstream os(fvp);
//while (simple.loop())
while (runTime.loop())
{
Info<< "Time = " << runTime.timeName() << nl << endl;
# include "readSIMPLEControls.H"
//modify processorBC
//identify patchID
label patchID_from = mesh.boundaryMesh().findPatchID("from");
label patchID_to = mesh.boundaryMesh().findPatchID("to");
//declaring Field<vector> given size
vectorField u1(1);
//surfaceVectorField second_exchange(vector(0,0,0));
//fvPatchField<vector> second_exchange = U.boundaryField()[patchID_to];
vectorField u2(1);
if (Pstream::myProcNo() == 0) {
//receive first_exchange from processor 1
IPstream::read(Pstream::blocking, 1,
reinterpret_cast<char*>(u2.begin()),
u2.byteSize());
//updating velocity at processor patch
fixedValueFvPatchVectorField& procVel =
refCast<fixedValueFvPatchVectorField>(U.boundaryField()[patchID_from]);
u1 = -(U.boundaryField()[patchID_from]).snGrad();
OPstream::write(Pstream::nonBlocking, 1,
reinterpret_cast<char*>(u1.begin()),
u1.byteSize());
lambda = (theta * u2 ) +
(1.0-theta)*lambda;
Pout<< "grad(u2): " << u1 << endl;
forAll(procVel, i)
{
procVel[i] = lambda[i];
}
//
}
else if (Pstream::myProcNo() == 1) {
void PatchGodunov::PLMNormalPred(FArrayBox& a_WMinus,
FArrayBox& a_WPlus,
const Real& a_dt,
const Real& a_dx,
const FArrayBox& a_W,
const FArrayBox& a_flat,
const int& a_dir,
const Box& a_box)
{
int numprim = m_gdnvPhysics->numPrimitives();
// This will hold 2nd or 4th order slopes
FArrayBox dW(a_box,numprim);
if (m_useFourthOrderSlopes)
{
// 2nd order slopes need to be computed over a larger box to accommodate
// the 4th order slope computation
Box boxVL = a_box;
boxVL.grow(a_dir,1);
boxVL &= m_domain;
// Compute 2nd order (van Leer) slopes
FArrayBox dWvL(boxVL, numprim);
m_util.vanLeerSlopes(dWvL,a_W,numprim,
m_useCharLimiting || m_usePrimLimiting,
a_dir,boxVL);
m_gdnvPhysics->getPhysIBC()->setBdrySlopes(dWvL,a_W,a_dir,m_currentTime);
// Compute 4th order slopes, without limiting.
m_util.fourthOrderSlopes(dW,a_W,dWvL, numprim, a_dir,a_box);
}
else
{
// Compute 2nd order (van Leer) slopes
m_util.vanLeerSlopes(dW,a_W,numprim,
m_useCharLimiting || m_usePrimLimiting,
a_dir,a_box);
m_gdnvPhysics->getPhysIBC()->setBdrySlopes(dW,a_W,a_dir,m_currentTime);
}
// To save on storage, we use the input values as temporaries for the
// delta's
a_WMinus.setVal(0.0);
a_WPlus .setVal(0.0);
if (m_useCharLimiting || m_useFourthOrderSlopes)
{
// Compute one-sided differences as inputs for limiting.
m_util.oneSidedDifferences(a_WMinus,a_WPlus,a_W,a_dir,a_box);
}
FArrayBox lambda(a_box, numprim);
m_gdnvPhysics->charValues(lambda, a_W, a_dir,a_box);
if (m_useCharLimiting && m_usePrimLimiting)
{
MayDay::Error("PatchGodunov::PLMNormalPred: Attempt to limit slopes in primitive AND characteristic coordinates - not implemented");
}
// Apply limiter on characteristic or primitive variables. Either
// way, must end up with characteristic variables to pass to to the
// normal predictor utility. Currently, cannot do both.
// If doing characteristic limiting then transform before limiting
if (m_useCharLimiting)
{
// Transform from primitive to characteristic variables
m_gdnvPhysics->charAnalysis(a_WMinus,a_W,a_dir,a_box);
m_gdnvPhysics->charAnalysis(a_WPlus ,a_W,a_dir,a_box);
m_gdnvPhysics->charAnalysis(dW ,a_W,a_dir,a_box);
}
if (m_useCharLimiting || m_usePrimLimiting)
{
// Limiting is already done for 2nd order slopes in primitive variables
// so don't do it again
if (m_useCharLimiting || m_useFourthOrderSlopes)
{
// Do slope limiting
m_util.slopeLimiter(dW,a_WMinus,a_WPlus,numprim,a_box);
}
// Do slope flattening
if (m_useFlattening)
{
m_util.applyFlattening(dW,a_flat,a_box);
}
}
// If not doing characteristic limiting then transform after any limiting
if (!m_useCharLimiting)
{
// Transform from primitive to characteristic variables
m_gdnvPhysics->charAnalysis(dW,a_W,a_dir,a_box);
}
// To the normal prediction in characteristic variables
m_util.PLMNormalPred(a_WMinus,a_WPlus,dW,lambda,a_dt / a_dx,a_box);
// Construct the increments to the primitive variables
m_gdnvPhysics->charSynthesis(a_WMinus,a_W,a_dir,a_box);
m_gdnvPhysics->charSynthesis(a_WPlus ,a_W,a_dir,a_box);
// Apply a physics-dependent post-normal predictor step:
// For example:
// - adjust/bound delta's so constraints on extrapolated primitive
// quantities are enforced (density and pressure > 0).
// - compute source terms that depend on the spatially varying
// coefficients.
m_gdnvPhysics->postNormalPred(a_WMinus,a_WPlus,a_W,a_dt,a_dx,a_dir,a_box);
// Compute the state from the increments
a_WMinus += a_W;
a_WPlus += a_W;
}
void PatchGodunov::PPMNormalPred(FArrayBox& a_WMinus,
FArrayBox& a_WPlus,
const Real& a_dt,
const Real& a_dx,
const FArrayBox& a_W,
const FArrayBox& a_flat,
const int& a_dir,
const Box& a_box)
{
int numprim = m_gdnvPhysics->numPrimitives();
Box faceBox = a_box;
// added by petermc, 22 Sep 2008:
// for 4th order, need extra faces in all the directions
if (m_highOrderLimiter) faceBox.grow(1);
faceBox.surroundingNodes(a_dir);
FArrayBox WFace(faceBox,numprim);
// Return WFace on face-centered faceBox.
m_util.PPMFaceValues(WFace,a_W,numprim,
m_useCharLimiting || m_usePrimLimiting,
a_dir,faceBox,m_currentTime,m_gdnvPhysics);
// To save on storage, we use the input values as temporaries for the
// delta's
a_WMinus.setVal(0.0);
a_WPlus .setVal(0.0);
a_WMinus -= a_W;
a_WPlus -= a_W;
WFace.shiftHalf(a_dir,1);
a_WMinus += WFace;
WFace.shift(a_dir,-1);
a_WPlus += WFace;
FArrayBox lambda(a_box, numprim);
m_gdnvPhysics->charValues(lambda, a_W, a_dir,a_box);
if (m_useCharLimiting && m_usePrimLimiting)
{
MayDay::Error("PatchGodunov::PPMNormalPred: Attempt to limit slopes in primitive AND characteristic coordinates - not implemented");
}
// Apply limiter on characteristic or primitive variables. Either
// way, must end up with characteristic variables to pass to to the
// normal predictor utility. Currently, cannot do both.
// If doing characteristic limiting then transform before limiting
if (m_useCharLimiting)
{
// Transform from primitive to characteristic variables
m_gdnvPhysics->charAnalysis(a_WMinus,a_W,a_dir,a_box);
m_gdnvPhysics->charAnalysis(a_WPlus ,a_W,a_dir,a_box);
}
if (m_useCharLimiting || m_usePrimLimiting)
{
// Do slope limiting
// m_util.PPMLimiter(a_WMinus,a_WPlus,numprim,a_box);
// petermc, 4 Sep 2008: included a_W and a_dir in argument list
m_util.PPMLimiter(a_WMinus, a_WPlus, a_W, numprim, a_dir, a_box);
// Do slope flattening
if (m_useFlattening)
{
m_util.applyFlattening(a_WMinus,a_flat,a_box);
m_util.applyFlattening(a_WPlus ,a_flat,a_box);
}
}
// If not doing characteristic limiting then transform after any limiting
if (!m_useCharLimiting)
{
// Transform from primitive to characteristic variables
m_gdnvPhysics->charAnalysis(a_WMinus,a_W,a_dir,a_box);
m_gdnvPhysics->charAnalysis(a_WPlus ,a_W,a_dir,a_box);
}
// To the normal prediction in characteristic variables
m_util.PPMNormalPred(a_WMinus,a_WPlus,lambda,a_dt / a_dx,numprim,a_box);
// Construct the increments to the primitive variables
m_gdnvPhysics->charSynthesis(a_WMinus,a_W,a_dir,a_box);
m_gdnvPhysics->charSynthesis(a_WPlus ,a_W,a_dir,a_box);
// Apply a physics-dependent post-normal predictor step:
// For example:
// - adjust/bound delta's so constraints on extrapolated primitive
// quantities are enforced (density and pressure > 0).
// - compute source terms that depend on the spatially varying
// coefficients.
m_gdnvPhysics->postNormalPred(a_WMinus,a_WPlus,a_W,a_dt,a_dx,a_dir,a_box);
// Compute the state from the increments
a_WMinus += a_W;
a_WPlus += a_W;
}
static int
disperse( /* check light sources for dispersion */
OBJREC *m,
RAY *r,
FVECT vt,
double tr,
COLOR cet,
COLOR abt
)
{
RAY sray;
const RAY *entray;
FVECT v1, v2, n1, n2;
FVECT dv, v2Xdv;
double v2Xdvv2Xdv;
int success = 0;
SRCINDEX si;
FVECT vtmp1, vtmp2;
double dtmp1, dtmp2;
int l1, l2;
COLOR ctmp;
int i;
/*
* This routine computes dispersion to the first order using
* the following assumptions:
*
* 1) The dependency of the index of refraction on wavelength
* is approximated by Hartmann's equation with lambda0
* equal to zero.
* 2) The entry and exit locations are constant with respect
* to dispersion.
*
* The second assumption permits us to model dispersion without
* having to sample refracted directions. We assume that the
* geometry inside the material is constant, and concern ourselves
* only with the relationship between the entering and exiting ray.
* We compute the first derivatives of the entering and exiting
* refraction with respect to the index of refraction. This
* is then used in a first order Taylor series to determine the
* index of refraction necessary to send the exiting ray to each
* light source.
* If an exiting ray hits a light source within the refraction
* boundaries, we sum all the frequencies over the disc of the
* light source to determine the resulting color. A smaller light
* source will therefore exhibit a sharper spectrum.
*/
if (!(r->crtype & REFRACTED)) { /* ray started in material */
VCOPY(v1, r->rdir);
n1[0] = -r->rdir[0]; n1[1] = -r->rdir[1]; n1[2] = -r->rdir[2];
} else {
/* find entry point */
for (entray = r; entray->rtype != REFRACTED;
entray = entray->parent)
;
entray = entray->parent;
if (entray->crtype & REFRACTED) /* too difficult */
return(0);
VCOPY(v1, entray->rdir);
VCOPY(n1, entray->ron);
}
VCOPY(v2, vt); /* exiting ray */
VCOPY(n2, r->ron);
/* first order dispersion approx. */
dtmp1 = 1./DOT(n1, v1);
dtmp2 = 1./DOT(n2, v2);
for (i = 0; i < 3; i++)
dv[i] = v1[i] + v2[i] - n1[i]*dtmp1 - n2[i]*dtmp2;
if (DOT(dv, dv) <= FTINY) /* null effect */
return(0);
/* compute plane normal */
fcross(v2Xdv, v2, dv);
v2Xdvv2Xdv = DOT(v2Xdv, v2Xdv);
/* check sources */
initsrcindex(&si);
while (srcray(&sray, r, &si)) {
if (DOT(sray.rdir, v2) < MINCOS)
continue; /* bad source */
/* adjust source ray */
dtmp1 = DOT(v2Xdv, sray.rdir) / v2Xdvv2Xdv;
sray.rdir[0] -= dtmp1 * v2Xdv[0];
sray.rdir[1] -= dtmp1 * v2Xdv[1];
sray.rdir[2] -= dtmp1 * v2Xdv[2];
l1 = lambda(m, v2, dv, sray.rdir); /* mean lambda */
if (l1 > MAXLAMBDA || l1 < MINLAMBDA) /* not visible */
continue;
/* trace source ray */
copycolor(sray.cext, cet);
copycolor(sray.albedo, abt);
normalize(sray.rdir);
rayvalue(&sray);
if (bright(sray.rcol) <= FTINY) /* missed it */
continue;
/*
* Compute spectral sum over diameter of source.
* First find directions for rays going to opposite
* sides of source, then compute wavelengths for each.
*/
fcross(vtmp1, v2Xdv, sray.rdir);
dtmp1 = sqrt(si.dom / v2Xdvv2Xdv / PI);
/* compute first ray */
VSUM(vtmp2, sray.rdir, vtmp1, dtmp1);
l1 = lambda(m, v2, dv, vtmp2); /* first lambda */
if (l1 < 0)
continue;
/* compute second ray */
VSUM(vtmp2, sray.rdir, vtmp1, -dtmp1);
l2 = lambda(m, v2, dv, vtmp2); /* second lambda */
if (l2 < 0)
continue;
/* compute color from spectrum */
if (l1 < l2)
spec_rgb(ctmp, l1, l2);
else
spec_rgb(ctmp, l2, l1);
multcolor(ctmp, sray.rcol);
scalecolor(ctmp, tr);
addcolor(r->rcol, ctmp);
success++;
}
return(success);
}
// main function
int main(int argc, const char * argv[])
{
gettimeofday(&start_timeval_rd1, NULL);
SparseMatrix<double> Gx_a(NX,NA);
Gx_a.resize(NX,NA);
SparseMatrix<double> Gx_b(NX,NB);
Gx_b.resize(NX,NB);
SparseMatrix<double> Gx_c(NX,NC);
Gx_c.resize(NX,NC);
Gx_a.makeCompressed();
Gx_b.makeCompressed();
Gx_c.makeCompressed();
// reading the partitions
Gx_a = read_G_sparse((char *) FILE_GA , "GX_A" ,NX, NA);
Gx_b = read_G_sparse((char *) FILE_GB , "GX_B" ,NX, NB);
Gx_c = read_G_sparse((char *) FILE_GC , "GX_C" ,NX, NC);
gettimeofday(&stop_timeval_rd1, NULL);
measure_stop_rd1 = stop_timeval_rd1.tv_usec + (timestamp_t)stop_timeval_rd1.tv_sec * 1000000;
measure_start_rd1 = start_timeval_rd1.tv_usec + (timestamp_t)start_timeval_rd1.tv_sec * 1000000;
time_rd1 = (measure_stop_rd1 - measure_start_rd1) / 1000000.0L;
printf("Exec Time reading matrices before preproc = %5.25e (Seconds)\n",time_rd1);
// initialize W, Z_B,Z_C, mu_a, mu_b, mu_c;
SparseMatrix<double> W(NA,KHID); W.resize(NA,KHID); W.makeCompressed();
SparseMatrix<double> Z_B(NA,NB); Z_B.resize(NA,NB); Z_B.makeCompressed();
SparseMatrix<double> Z_C(NA,NC); Z_C.resize(NA,NC); Z_C.makeCompressed();
VectorXd mu_a(NA);
VectorXd mu_b(NB);
VectorXd mu_c(NC);
cout << "----------------------------Before whitening--------------------------" << endl;
gettimeofday(&start_timeval_pre, NULL); // measuring start time for pre processing
second_whiten(Gx_a,Gx_b,Gx_c,W,Z_B,Z_C,mu_a,mu_b,mu_c);
// whitened datapoints
SparseMatrix<double> Data_a_G = W.transpose() * Gx_a.transpose();
VectorXd Data_a_mu = W.transpose() * mu_a;
SparseMatrix<double> Data_b_G = W.transpose() * Z_B * Gx_b.transpose();
VectorXd Data_b_mu = W.transpose() * Z_B * mu_b;
SparseMatrix<double> Data_c_G = W.transpose() * Z_C * Gx_c.transpose();
VectorXd Data_c_mu = W.transpose() * Z_C * mu_c;
gettimeofday(&stop_timeval_pre, NULL); // measuring stop time for pre processing
measure_stop_pre = stop_timeval_pre.tv_usec + (timestamp_t)stop_timeval_pre.tv_sec * 1000000;
measure_start_pre = start_timeval_pre.tv_usec + (timestamp_t)start_timeval_pre.tv_sec * 1000000;
time_pre = (measure_stop_pre - measure_start_pre) / 1000000.0L;
printf("time taken by preprocessing = %5.25e (Seconds)\n",time_pre);
cout << "----------------------------After whitening---------------------------" << endl;
// stochastic updates
VectorXd lambda(KHID);
MatrixXd phi_new(KHID,KHID);
cout << "------------------------------Before tensor decomposition----------------" << endl;
gettimeofday(&start_timeval_stpm, NULL); // measuring start time for stochastic updates
tensorDecom_alpha0(Data_a_G,Data_a_mu,Data_b_G,Data_b_mu,Data_c_G,Data_c_mu,lambda,phi_new);
gettimeofday(&stop_timeval_stpm, NULL); // measuring stop time for stochastic updates
cout << "after tensor decomposition" << endl;
measure_stop_stpm = stop_timeval_stpm.tv_usec + (timestamp_t)stop_timeval_stpm.tv_sec * 1000000;
measure_start_stpm = start_timeval_stpm.tv_usec + (timestamp_t)start_timeval_stpm.tv_sec * 1000000;
time_stpm = (measure_stop_stpm - measure_start_stpm) / 1000000.0L;
cout << "------------------------------After tensor decomposition----------------" << endl;
printf("time taken by stochastic tensor decomposition = %5.25e (Seconds)\n",time_stpm);
//cout << phi_new << endl; // optionally print eigenvectors
cout << "the eigenvalues are:" << endl;
cout << lambda << endl;
// post processing
cout << "------------Reading Gb_a, Gc_a---------"<<endl;
gettimeofday(&start_timeval_rd2, NULL);
#ifdef CalErrALL
// read the matrix Gab and Gac
SparseMatrix<double> Gb_a(NB,NA);Gb_a.resize(NB,NA);
SparseMatrix<double> Gc_a(NC,NA);Gc_a.resize(NC,NA);
Gb_a = read_G_sparse((char *) FILE_Gb_a, "GB_A" ,NB, NA); Gb_a.makeCompressed();
Gc_a = read_G_sparse((char *) FILE_Gc_a ,"GC_A" ,NC, NA); Gc_a.makeCompressed();
// releasing memory of Gx_a, Gx_b, Gx_c;
Gx_b.resize(0,0);Gx_c.resize(0,0);
#endif
MatrixXd Inv_Lambda = (pinv_vector(lambda)).asDiagonal();
SparseMatrix<double> inv_lam_phi = (Inv_Lambda.transpose() * phi_new.transpose()).sparseView();
gettimeofday(&stop_timeval_rd2, NULL);
measure_stop_rd2 = stop_timeval_rd2.tv_usec + (timestamp_t)stop_timeval_rd2.tv_sec * 1000000;
measure_start_rd2 = start_timeval_rd2.tv_usec + (timestamp_t)start_timeval_rd2.tv_sec * 1000000;
time_rd2 = (measure_stop_rd2 - measure_start_rd2) / 1000000.0L;
cout << "------------After reading Gb_a, Gc_a---------"<<endl;
printf("time taken for reading matrices after post processing = %5.25e (Seconds)\n",time_rd2);
cout << "---------------------------Computing pi matrices-----------------------------" << endl;
gettimeofday(&start_timeval_post, NULL); // measuring start time for post processing
SparseMatrix<double> pi_x(KHID,NX);pi_x.reserve(KHID*NX);pi_x.makeCompressed();
SparseMatrix<double> pi_x_tmp1 = inv_lam_phi * W.transpose();
#ifdef CalErrALL
SparseMatrix<double> pi_a(KHID,NA);pi_a.reserve(KHID*NA);pi_a.makeCompressed();
SparseMatrix<double> pi_b(KHID,NB);pi_b.reserve(KHID*NB);pi_b.makeCompressed();
SparseMatrix<double> pi_c(KHID,NC);pi_c.reserve(KHID*NC);pi_c.makeCompressed();
pi_a = pi_x_tmp1 * Z_B * Gb_a;
MatrixXd pi_a_full = (MatrixXd) pi_a;pi_a.resize(0,0);
pi_b = pi_x_tmp1 * Gb_a.transpose();
MatrixXd pi_b_full = (MatrixXd) pi_b;pi_b.resize(0,0);
pi_c = pi_x_tmp1 * Gc_a.transpose();
MatrixXd pi_c_full = (MatrixXd) pi_c;pi_c.resize(0,0);
#endif
pi_x =pi_x_tmp1 * Gx_a.transpose();Gx_a.resize(0,0);
MatrixXd pi_x_full = (MatrixXd) pi_x;pi_x.resize(0,0);
gettimeofday(&stop_timeval_post, NULL); // measuring stop time for post processing
measure_stop_post = stop_timeval_post.tv_usec + (timestamp_t)stop_timeval_post.tv_sec * 1000000;
measure_start_post = start_timeval_post.tv_usec + (timestamp_t)start_timeval_post.tv_sec * 1000000;
time_post = (measure_stop_post - measure_start_post) / 1000000.0L;
cout << "---------After post processing------------" << endl;
printf("time taken for post processing = %5.25e (Seconds)\n",time_post);
cout<<"-------------------------Concatenation for pi_est-------------------- "<< endl;
// store true_pi
#ifdef CalErrALL
long PI_LEN =(long) NX+NA+NB+NC;
#else
long PI_LEN =(long) NX;
#endif
MatrixXd My_pi_true_mat(KTRUE,PI_LEN);
MatrixXd My_pi_est_mat(KHID,PI_LEN);
#ifdef CalErrALL
for (int kk = 0; kk < KHID; kk++)
{
// for My_pi_est;
VectorXd My_pi_est1(NX+NA);
My_pi_est1 = concatenation_vector (pi_x_full.row(kk), pi_a_full.row(kk));
VectorXd My_pi_est2(NX+NA+NB);
My_pi_est2 =concatenation_vector (My_pi_est1, pi_b_full.row(kk));
VectorXd My_pi_est3(NX+NA+NB+NC);
My_pi_est3 =concatenation_vector (My_pi_est2, pi_c_full.row(kk));
My_pi_est_mat.row(kk) = My_pi_est3;
}
pi_a_full.resize(0,0);
pi_b_full.resize(0,0);
pi_c_full.resize(0,0);
#else
My_pi_est_mat =pi_x_full;
#endif
pi_x_full.resize(0,0);
// converting them to stochastic matrix
My_pi_est_mat = normProbMatrix(My_pi_est_mat);
SparseMatrix<double> sparse_my_pi_est_mat = My_pi_est_mat.sparseView();
cout << "-----------Before writing results: W, Z_B,Z_C and pi-----------"<<endl;
write_pi(FILE_PI_WRITE, sparse_my_pi_est_mat);
write_pi(FILE_WHITE_WRITE, W);
write_pi(FILE_INVLAMPHI_WRITE, inv_lam_phi);
cout << "-----------After writing results---------"<< endl;
#ifdef ErrCal // set error calculation flag if it needs to be computed
cout << "--------------------------------Calculating error------------------------------" << endl;
gettimeofday(&start_timeval_error, NULL); // measuring start time for error calculation
#ifdef CalErrALL
// calculate error
Gb_a.resize(0,0); Gc_a.resize(0,0);
// read pi_true, i.e., ground truth matrices
SparseMatrix<double> Pi_true_a(KTRUE,NA);Pi_true_a.makeCompressed();Pi_true_a = read_G_sparse((char *) FILE_Pi_a , "Pi_true_A" ,KTRUE, NA);
MatrixXd Pi_true_a_full = (MatrixXd) Pi_true_a; Pi_true_a.resize(0,0);
SparseMatrix<double> Pi_true_b(KTRUE,NB);Pi_true_b.makeCompressed();Pi_true_b = read_G_sparse((char *) FILE_Pi_b , "Pi_true_B" ,KTRUE, NB);
MatrixXd Pi_true_b_full = (MatrixXd) Pi_true_b; Pi_true_b.resize(0,0);
SparseMatrix<double> Pi_true_c(KTRUE,NC);Pi_true_c.makeCompressed();Pi_true_c = read_G_sparse((char *) FILE_Pi_c , "Pi_true_C" ,KTRUE, NC);
MatrixXd Pi_true_c_full = (MatrixXd) Pi_true_c; Pi_true_c.resize(0,0);
#endif
SparseMatrix<double> Pi_true_x(KTRUE,NX);Pi_true_x.makeCompressed();Pi_true_x = read_G_sparse((char *) FILE_Pi_x , "Pi_true_X" ,KTRUE, NX);
MatrixXd Pi_true_x_full = (MatrixXd) Pi_true_x; Pi_true_x.resize(0,0);
/*
// this is only for yelp, comment this for DBLP
long PI_LEN = (long)NX;
MatrixXd My_pi_true_mat(KTRUE,PI_LEN);
My_pi_true_mat = Pi_true_x_full;
MatrixXd My_pi_est_mat(KHID,PI_LEN);
My_pi_est_mat = pi_x_full;
*/
cout<<"-------------------------Concatenation for pi_true-------------------- "<< endl;
#ifdef CalErrALL
for ( int k = 0; k < KTRUE; k++)
{
// for My_pi_true;
VectorXd My_pi_true1(NX+NA);
My_pi_true1 = concatenation_vector ((Pi_true_x_full.row(k)),(Pi_true_a_full.row(k)));
VectorXd My_pi_true2(NX+NA+NB);
My_pi_true2 =concatenation_vector (My_pi_true1, (Pi_true_b_full.row(k)));
VectorXd My_pi_true3(NX+NA+NB+NC);
My_pi_true3 =concatenation_vector (My_pi_true2, (Pi_true_c_full.row(k)));
My_pi_true_mat.row(k) = My_pi_true3;
}
Pi_true_a_full.resize(0,0);
Pi_true_b_full.resize(0,0);
Pi_true_c_full.resize(0,0);
#else
My_pi_true_mat = Pi_true_x_full;
#endif
Pi_true_x_full.resize(0,0);
double thresh_vec[NUM_THRESH] = thresh_vec_def;
//{0.3, 0.25, 0.2, 0.18, 0.15, 0.12, 0.1,0.08};
double error_vec[NUM_THRESH] = {0.0};
double match_vec[NUM_THRESH] = {0.0};
for (int tttt = 0; tttt < NUM_THRESH; tttt++)
{
MatrixXd p_values=MatrixXd::Zero(KTRUE,KHID);
MatrixXd errors=MatrixXd::Zero(KTRUE,KHID);
for ( int k = 0; k < KTRUE; k++)
{
VectorXd my_pi_true_eigen = My_pi_true_mat.row(k);
double *my_pi_true = my_pi_true_eigen.data();
for (int kk = 0; kk < KHID; kk++)
{
VectorXd my_pi_est_eigen = My_pi_est_mat.row(kk);
double *my_pi_est = my_pi_est_eigen.data();
for(long lltt = 0; lltt < PI_LEN; lltt++)
{
if(my_pi_est[lltt] < thresh_vec[tttt])
my_pi_est[lltt] = 0;
else
my_pi_est[lltt] = 1;
}
// calculate p-values and error
double correlation = Calculate_Correlation(my_pi_est, my_pi_true, (long)PI_LEN); //{long}
if (correlation > 0)
{
p_values(k,kk)=Calculate_Pvalue(my_pi_true, my_pi_est, (long)PI_LEN); //{long}
if (p_values(k,kk) < PVALUE_TOLE)
{
errors(k,kk)=(my_pi_true_eigen - my_pi_est_eigen).cwiseAbs().sum();
}
else
{
errors(k,kk)=0;
}
}
else
{
p_values(k,kk)=-1;
errors(k,kk)=0;
}
}
}
VectorXd matched = errors.rowwise().sum();
double nnz =0;
for(long calc=0; calc <KTRUE; calc++)
{
if(matched(calc)>0)
nnz++;
}
error_vec[tttt]=(double)errors.sum()/((double)PI_LEN*KTRUE);
match_vec[tttt]=((double)nnz)/((double)KTRUE);
}
gettimeofday(&stop_timeval_error, NULL); // measuring stop time for error calculation
measure_stop_error = stop_timeval_error.tv_usec + (timestamp_t)stop_timeval_error.tv_sec * 1000000;
measure_start_error = start_timeval_error.tv_usec + (timestamp_t)start_timeval_error.tv_sec * 1000000;
time_error = (measure_stop_error - measure_start_error) / 1000000.0L;
cout << "---------After error calculation------------"<<endl;
printf("time taken for error calculation = %5.25e (Seconds)\n",time_error);
furongprintVector(thresh_vec, NUM_THRESH, "thresh vector "); // outputs are printed
furongprintVector(error_vec, NUM_THRESH, "error vector ");
furongprintVector(match_vec, NUM_THRESH, "match vector ");
#endif
cout << "Program over" << endl;
printf("\ntime taken for execution of the whole program = %5.25e (Seconds)\n", time_rd1 + time_pre + time_stpm + time_rd2 + time_post);
return 0;
}
Object* Proc::call(STATE, Arguments& args) {
bool lambda_style = CBOOL(lambda());
int flags = 0;
Proc* self = this;
OnStack<1> os(state, self);
// Check the arity in lambda mode
if(lambda_style && !block()->nil_p()) {
flags = CallFrame::cIsLambda;
int total = self->block()->compiled_code()->total_args()->to_native();
int required = self->block()->compiled_code()->required_args()->to_native();
bool arity_ok = false;
if(Fixnum* fix = try_as<Fixnum>(self->block()->compiled_code()->splat())) {
switch(fix->to_native()) {
case -2:
arity_ok = true;
break;
case -4:
// splat = -4 means { |(a, b)| }
if(args.total() == 1) {
Array* ary = 0;
Object* obj = args.get_argument(0);
if(!(ary = try_as<Array>(obj))) {
if(CBOOL(obj->respond_to(state, G(sym_to_ary), cFalse))) {
if(!(ary = try_as<Array>(obj->send(state, G(sym_to_ary))))) {
Exception::type_error(state, "to_ary must return an Array");
return 0;
}
}
}
if(ary) args.use_argument(ary);
}
// fall through for arity check
case -3:
// splat = -3 is used to distinguish { |a, | } from { |a| }
if(args.total() == (size_t)required) arity_ok = true;
break;
default:
if(args.total() >= (size_t)required) {
arity_ok = true;
}
}
} else {
arity_ok = args.total() <= (size_t)total &&
args.total() >= (size_t)required;
}
if(!arity_ok) {
Exception* exc =
Exception::make_argument_error(state, required, args.total(),
block()->compiled_code()->name());
exc->locations(state, Location::from_call_stack(state));
state->raise_exception(exc);
return NULL;
}
}
if(self->bound_method()->nil_p()) {
if(self->block()->nil_p()) {
Dispatch dispatch(state->symbol("__yield__"));
return dispatch.send(state, args);
} else {
return self->block()->call(state, args, flags);
}
} else if(NativeMethod* nm = try_as<NativeMethod>(self->bound_method())) {
return nm->execute(state, nm, G(object), args);
} else if(NativeFunction* nf = try_as<NativeFunction>(self->bound_method())) {
return nf->call(state, args);
} else {
Exception* exc =
Exception::make_type_error(state, BlockEnvironment::type, self->bound_method(), "NativeMethod nor NativeFunction bound to proc");
exc->locations(state, Location::from_call_stack(state));
state->raise_exception(exc);
return NULL;
}
}
/**
* This function will return a lambda closure that, upon execution, will
* return the array used to construct the same closure. The new closure will
* take the specified args, which, when evaluated, will end up as constants
* in the newly compiled closure. All other symbols found in the input array
* will appear in the new lambda closure unquoted one level.
*
* Example:
* <code><pre>
* funcall(reconstruct_lambda(({ 'arg })),
({ #'+, 'arg, ''s })), "str")</pre></code>
* returns:
* <code><pre>({ #'+, "str", 's})</pre></code>
*
* @param args an array of arguments to the new lambda closure
* @param arr the closure which is to be reconstructed
* @return a closure which, when executed, should return arr
*/
closure reconstruct_lambda(symbol *args, mixed *arr) {
return lambda(args, _reconstruct(arr));
}
// Coordinate descent for logistic models
RcppExport SEXP cdfit_binomial_hsr(SEXP X_, SEXP y_, SEXP row_idx_,
SEXP lambda_, SEXP nlambda_, SEXP lam_scale_,
SEXP lambda_min_, SEXP alpha_, SEXP user_, SEXP eps_,
SEXP max_iter_, SEXP multiplier_, SEXP dfmax_,
SEXP ncore_, SEXP warn_, SEXP verbose_) {
XPtr<BigMatrix> xMat(X_);
double *y = REAL(y_);
int *row_idx = INTEGER(row_idx_);
double lambda_min = REAL(lambda_min_)[0];
double alpha = REAL(alpha_)[0];
int n = Rf_length(row_idx_); // number of observations used for fitting model
int p = xMat->ncol();
int L = INTEGER(nlambda_)[0];
int lam_scale = INTEGER(lam_scale_)[0];
double eps = REAL(eps_)[0];
int max_iter = INTEGER(max_iter_)[0];
double *m = REAL(multiplier_);
int dfmax = INTEGER(dfmax_)[0];
int warn = INTEGER(warn_)[0];
int user = INTEGER(user_)[0];
int verbose = INTEGER(verbose_)[0];
NumericVector lambda(L);
NumericVector Dev(L);
IntegerVector iter(L);
IntegerVector n_reject(L);
NumericVector beta0(L);
NumericVector center(p);
NumericVector scale(p);
int p_keep = 0; // keep columns whose scale > 1e-6
int *p_keep_ptr = &p_keep;
vector<int> col_idx;
vector<double> z;
double lambda_max = 0.0;
double *lambda_max_ptr = &lambda_max;
int xmax_idx = 0;
int *xmax_ptr = &xmax_idx;
// set up omp
int useCores = INTEGER(ncore_)[0];
#ifdef BIGLASSO_OMP_H_
int haveCores = omp_get_num_procs();
if(useCores < 1) {
useCores = haveCores;
}
omp_set_dynamic(0);
omp_set_num_threads(useCores);
#endif
if (verbose) {
char buff1[100];
time_t now1 = time (0);
strftime (buff1, 100, "%Y-%m-%d %H:%M:%S.000", localtime (&now1));
Rprintf("\nPreprocessing start: %s\n", buff1);
}
// standardize: get center, scale; get p_keep_ptr, col_idx; get z, lambda_max, xmax_idx;
standardize_and_get_residual(center, scale, p_keep_ptr, col_idx, z, lambda_max_ptr, xmax_ptr, xMat,
y, row_idx, lambda_min, alpha, n, p);
p = p_keep; // set p = p_keep, only loop over columns whose scale > 1e-6
if (verbose) {
char buff1[100];
time_t now1 = time (0);
strftime (buff1, 100, "%Y-%m-%d %H:%M:%S.000", localtime (&now1));
Rprintf("Preprocessing end: %s\n", buff1);
Rprintf("\n-----------------------------------------------\n");
}
arma::sp_mat beta = arma::sp_mat(p, L); //beta
double *a = Calloc(p, double); //Beta from previous iteration
double a0 = 0.0; //beta0 from previousiteration
double *w = Calloc(n, double);
double *s = Calloc(n, double); //y_i - pi_i
double *eta = Calloc(n, double);
int *e1 = Calloc(p, int); //ever-active set
int *e2 = Calloc(p, int); //strong set
double xwr, xwx, pi, u, v, cutoff, l1, l2, shift, si;
double max_update, update, thresh; // for convergence check
int i, j, jj, l, violations, lstart;
double ybar = sum(y, n) / n;
a0 = beta0[0] = log(ybar / (1-ybar));
double nullDev = 0;
double *r = Calloc(n, double);
for (i = 0; i < n; i++) {
r[i] = y[i];
nullDev = nullDev - y[i]*log(ybar) - (1-y[i])*log(1-ybar);
s[i] = y[i] - ybar;
eta[i] = a0;
}
thresh = eps * nullDev / n;
double sumS = sum(s, n); // temp result sum of s
double sumWResid = 0.0; // temp result: sum of w * r
// set up lambda
if (user == 0) {
if (lam_scale) { // set up lambda, equally spaced on log scale
double log_lambda_max = log(lambda_max);
double log_lambda_min = log(lambda_min*lambda_max);
double delta = (log_lambda_max - log_lambda_min) / (L-1);
for (l = 0; l < L; l++) {
lambda[l] = exp(log_lambda_max - l * delta);
}
} else { // equally spaced on linear scale
double delta = (lambda_max - lambda_min*lambda_max) / (L-1);
for (l = 0; l < L; l++) {
lambda[l] = lambda_max - l * delta;
}
}
Dev[0] = nullDev;
lstart = 1;
n_reject[0] = p;
} else {
lstart = 0;
lambda = Rcpp::as<NumericVector>(lambda_);
}
for (l = lstart; l < L; l++) {
if(verbose) {
// output time
char buff[100];
time_t now = time (0);
strftime (buff, 100, "%Y-%m-%d %H:%M:%S.000", localtime (&now));
Rprintf("Lambda %d. Now time: %s\n", l, buff);
}
if (l != 0) {
// Check dfmax
int nv = 0;
for (j = 0; j < p; j++) {
if (a[j] != 0) {
nv++;
}
}
if (nv > dfmax) {
for (int ll=l; ll<L; ll++) iter[ll] = NA_INTEGER;
Free_memo_bin_hsr(s, w, a, r, e1, e2, eta);
return List::create(beta0, beta, center, scale, lambda, Dev,
iter, n_reject, Rcpp::wrap(col_idx));
}
// strong set
cutoff = 2*lambda[l] - lambda[l-1];
for (j = 0; j < p; j++) {
if (fabs(z[j]) > (cutoff * alpha * m[col_idx[j]])) {
e2[j] = 1;
} else {
e2[j] = 0;
}
}
} else {
// strong set
cutoff = 2*lambda[l] - lambda_max;
for (j = 0; j < p; j++) {
if (fabs(z[j]) > (cutoff * alpha * m[col_idx[j]])) {
e2[j] = 1;
} else {
e2[j] = 0;
}
}
}
n_reject[l] = p - sum(e2, p);
while (iter[l] < max_iter) {
while (iter[l] < max_iter) {
while (iter[l] < max_iter) {
iter[l]++;
Dev[l] = 0.0;
for (i = 0; i < n; i++) {
if (eta[i] > 10) {
pi = 1;
w[i] = .0001;
} else if (eta[i] < -10) {
pi = 0;
w[i] = .0001;
} else {
pi = exp(eta[i]) / (1 + exp(eta[i]));
w[i] = pi * (1 - pi);
}
s[i] = y[i] - pi;
r[i] = s[i] / w[i];
if (y[i] == 1) {
Dev[l] = Dev[l] - log(pi);
} else {
Dev[l] = Dev[l] - log(1-pi);
}
}
if (Dev[l] / nullDev < .01) {
if (warn) warning("Model saturated; exiting...");
for (int ll=l; ll<L; ll++) iter[ll] = NA_INTEGER;
Free_memo_bin_hsr(s, w, a, r, e1, e2, eta);
return List::create(beta0, beta, center, scale, lambda, Dev,
iter, n_reject, Rcpp::wrap(col_idx));
}
// Intercept
xwr = crossprod(w, r, n, 0);
xwx = sum(w, n);
beta0[l] = xwr / xwx + a0;
si = beta0[l] - a0;
if (si != 0) {
a0 = beta0[l];
for (i = 0; i < n; i++) {
r[i] -= si; //update r
eta[i] += si; //update eta
}
}
sumWResid = wsum(r, w, n); // update temp result: sum of w * r, used for computing xwr;
max_update = 0.0;
for (j = 0; j < p; j++) {
if (e1[j]) {
jj = col_idx[j];
xwr = wcrossprod_resid(xMat, r, sumWResid, row_idx, center[jj], scale[jj], w, n, jj);
v = wsqsum_bm(xMat, w, row_idx, center[jj], scale[jj], n, jj) / n;
u = xwr/n + v * a[j];
l1 = lambda[l] * m[jj] * alpha;
l2 = lambda[l] * m[jj] * (1-alpha);
beta(j, l) = lasso(u, l1, l2, v);
shift = beta(j, l) - a[j];
if (shift !=0) {
// update change of objective function
// update = - u * shift + (0.5 * v + 0.5 * l2) * (pow(beta(j, l), 2) - pow(a[j], 2)) + l1 * (fabs(beta(j, l)) - fabs(a[j]));
update = pow(beta(j, l) - a[j], 2) * v;
if (update > max_update) max_update = update;
update_resid_eta(r, eta, xMat, shift, row_idx, center[jj], scale[jj], n, jj); // update r
sumWResid = wsum(r, w, n); // update temp result w * r, used for computing xwr;
a[j] = beta(j, l); // update a
}
}
}
// Check for convergence
if (max_update < thresh) break;
}
// Scan for violations in strong set
sumS = sum(s, n);
violations = check_strong_set_bin(e1, e2, z, xMat, row_idx, col_idx, center, scale, a, lambda[l], sumS, alpha, s, m, n, p);
if (violations==0) break;
}
// Scan for violations in rest
violations = check_rest_set_bin(e1, e2, z, xMat, row_idx, col_idx, center, scale, a, lambda[l], sumS, alpha, s, m, n, p);
if (violations==0) break;
}
}
Free_memo_bin_hsr(s, w, a, r, e1, e2, eta);
return List::create(beta0, beta, center, scale, lambda, Dev, iter, n_reject, Rcpp::wrap(col_idx));
}
int main(int argc, char* argv[])
{
// Define nonlinear thermal conductivity lambda(u) via a cubic spline.
// Step 1: Fill the x values and use lambda_macro(u) = 1 + u^4 for the y values.
#define lambda_macro(x) (1 + Hermes::pow(x, 4))
Hermes::vector<double> lambda_pts(-2.0, -1.5, -1.0, -0.5, 0.0, 0.5, 1.0, 1.5, 2.0, 2.5, 3.0, 4.0, 5.0);
Hermes::vector<double> lambda_val;
for (unsigned int i = 0; i < lambda_pts.size(); i++)
lambda_val.push_back(lambda_macro(lambda_pts[i]));
// Step 2: Create the cubic spline (and plot it for visual control).
double bc_left = 0.0;
double bc_right = 0.0;
bool first_der_left = false;
bool first_der_right = false;
bool extrapolate_der_left = true;
bool extrapolate_der_right = true;
CubicSpline lambda(lambda_pts, lambda_val, bc_left, bc_right, first_der_left, first_der_right,
extrapolate_der_left, extrapolate_der_right);
info("Saving cubic spline into a Pylab file spline.dat.");
double interval_extension = 3.0; // The interval of definition of the spline will be
// extended by "interval_extension" on both sides.
lambda.plot("spline.dat", interval_extension);
// Load the mesh.
Mesh mesh;
MeshReaderH2D mloader;
mloader.load("square.mesh", &mesh);
// Perform initial mesh refinements.
for(int i = 0; i < INIT_GLOB_REF_NUM; i++) mesh.refine_all_elements();
mesh.refine_towards_boundary("Bdy", INIT_BDY_REF_NUM);
// Initialize boundary conditions.
CustomEssentialBCNonConst bc_essential("Bdy");
EssentialBCs<double> bcs(&bc_essential);
// Create an H1 space with default shapeset.
H1Space<double> space(&mesh, &bcs, P_INIT);
int ndof = space.get_num_dofs();
info("ndof: %d", ndof);
// Initialize the weak formulation.
Hermes2DFunction<double> src(-heat_src);
DefaultWeakFormPoisson<double> wf(HERMES_ANY, &lambda, &src);
// Initialize the FE problem.
DiscreteProblem<double> dp(&wf, &space);
// Project the initial condition on the FE space to obtain initial
// coefficient vector for the Newton's method.
// NOTE: If you want to start from the zero vector, just define
// coeff_vec to be a vector of ndof zeros (no projection is needed).
info("Projecting to obtain initial vector for the Newton's method.");
double* coeff_vec = new double[ndof];
CustomInitialCondition init_sln(&mesh);
OGProjection<double>::project_global(&space, &init_sln, coeff_vec, matrix_solver);
// Initialize Newton solver.
NewtonSolver<double> newton(&dp, matrix_solver);
// Perform Newton's iteration.
bool freeze_jacobian = false;
if (!newton.solve(coeff_vec, NEWTON_TOL, NEWTON_MAX_ITER, freeze_jacobian))
error("Newton's iteration failed.");
// Translate the resulting coefficient vector into a Solution.
Solution<double> sln;
Solution<double>::vector_to_solution(newton.get_sln_vector(), &space, &sln);
// Get info about time spent during assembling in its respective parts.
//dp.get_all_profiling_output(std::cout);
// Clean up.
delete [] coeff_vec;
// Visualise the solution and mesh.
ScalarView s_view("Solution", new WinGeom(0, 0, 440, 350));
s_view.show_mesh(false);
s_view.show(&sln);
OrderView<double> o_view("Mesh", new WinGeom(450, 0, 400, 350));
o_view.show(&space);
// Wait for all views to be closed.
View::wait();
return 0;
}
void LocalMaxStatUtil::descendingLadderEpochRepeat (
size_t dimension_, // #(distinct values)
const Int4 *score_, // values
const double *prob_, // probability of corresponding value
double *eSumAlpha_, // expectation (sum [alpha])
double *eOneMinusExpSumAlpha_, // expectation [1.0 - exp (sum [alpha])]
bool isStrict_, // ? is this a strict descending ladder epoch
double lambda_, // lambda for repeats : default is lambda0_ below
size_t endW_, // maximum w plus 1
double *pAlphaW_, // probability {alpha = w} : pAlphaW_ [0, wEnd)
double *eOneMinusExpSumAlphaW_, // expectation [1.0 - exp (sum [alpha]); alpha = w] : eOneMinusExpSumAlphaW_ [0, wEnd)
double lambda0_, // lambda for flattened distribution (avoid recomputation)
double mu0_, // mean of flattened distribution (avoid recomputation)
double muAssoc0_, // mean of associated flattened distribution (avoid recomputation)
double thetaMin0_, // thetaMin of flattened distribution (avoid recomputation)
double rMin0_, // rMin of flattened distribution (avoid recomputation)
double time_, // get time for the dynamic programming computation
bool *terminated_) // ? Was the dynamic programming computation terminated prematurely ?
// assumes logarithmic regime
{
// Start dynamic programming probability calculation using notation in
//
// Mott R. and Tribe R. (1999)
// J. Computational Biology 6(1):91-112
//
// Karlin S. and Taylor H.M.(1981)
// A Second Course in Stochastic Processes, p. 480
//
// Note there is an error in Eq (6.19) there, which is corrected in Eq (6.20)
//
// This program uses departure into (-Inf, 0] not (-Inf, 0)
// avoid recomputation
#ifdef _DEBUG /* to eliminate compiler warning in release mode */
double mu0 = 0.0 == mu0_ ? mu (dimension_, score_, prob_) : mu0_;
#endif
_ASSERT(mu0 < 0.0);
double lambda0 = 0.0 == lambda0_ ? lambda (dimension_, score_, prob_) : lambda0_;
_ASSERT (0.0 < lambda0);
if (lambda_ == 0.0) lambda_ = lambda0;
_ASSERT (0.0 < lambda_);
#ifdef _DEBUG /* to eliminate compiler warning in release mode */
double muAssoc0 = 0.0 == muAssoc0_ ? muAssoc (dimension_, score_, prob_, lambda0) : muAssoc0_;
#endif
_ASSERT (0.0 < muAssoc0);
double thetaMin0 = 0.0 == thetaMin0_ ? thetaMin (dimension_, score_, prob_, lambda0) : thetaMin0_;
_ASSERT (0.0 < thetaMin0);
double rMin0 = 0.0 == rMin0_ ? rMin (dimension_, score_, prob_, lambda0, thetaMin0) : rMin0_;
_ASSERT (0.0 < rMin0 && rMin0 < 1.0);
const Int4 ITER_MIN = static_cast <Int4> ((log (REL_TOL * (1.0 - rMin0)) / log (rMin0)));
_ASSERT (0 < ITER_MIN);
const Int4 ITER = static_cast <Int4> (endW_) < ITER_MIN ? ITER_MIN : static_cast <Int4> (endW_);
_ASSERT (0 < ITER);
const Int4 Y_MAX = static_cast <Int4> (-log (REL_TOL) / lambda0);
Int4 entry = isStrict_ ? -1 : 0;
n_setParameters (dimension_, score_, prob_, entry);
double time0 = 0.0;
double time1 = 0.0;
if (time_ > 0.0) Sls::alp_data::get_current_time (time0);
DynProgProbLim dynProgProb (n_step, dimension_, prob_, score_ [0] - 1, Y_MAX);
if (pAlphaW_) pAlphaW_ [0] = 0.0;
if (eOneMinusExpSumAlphaW_) eOneMinusExpSumAlphaW_ [0] = 0.0;
dynProgProb.update (); // iterate random walk
Int4 value = 0;
if (eSumAlpha_) *eSumAlpha_ = 0.0;
if (eOneMinusExpSumAlpha_) *eOneMinusExpSumAlpha_ = 0.0;
for (size_t w = 1; w < static_cast <size_t> (ITER); w++) {
if (w < endW_) { // sum pAlphaW_ [w] and eOneMinusExpSumAlphaW_ [w]
if (pAlphaW_) pAlphaW_ [w] = 0.0;
if (eOneMinusExpSumAlphaW_) eOneMinusExpSumAlphaW_ [w] = 0.0;
for (value = score_ [0]; value <= entry; value++) {
if (pAlphaW_) pAlphaW_ [w] += dynProgProb.getProb (value);
if (eOneMinusExpSumAlphaW_) eOneMinusExpSumAlphaW_ [w] +=
dynProgProb.getProb (value) *
(1.0 - exp (lambda_ * static_cast <double> (value)));
}
}
for (value = score_ [0]; value <= entry; value++) {
if (eSumAlpha_) *eSumAlpha_ += dynProgProb.getProb (value) * static_cast <double> (value);
if (eOneMinusExpSumAlpha_) *eOneMinusExpSumAlpha_ += dynProgProb.getProb (value) *
(1.0 - exp (lambda_ * static_cast <double> (value)));
}
dynProgProb.setValueFct (n_bury);
dynProgProb.update (); // put probability into the morgue
dynProgProb.setValueFct (n_step);
dynProgProb.update (); // iterate random walk
if (time_ > 0.0)
{
Sls::alp_data::get_current_time (time1);
if (time1 - time0 > time_)
{
*terminated_ = true;
return;
}
}
}
for (value = score_ [0]; value <= entry; value++) {
if (eSumAlpha_) *eSumAlpha_ += dynProgProb.getProb (value) * static_cast <double> (value);
if (eOneMinusExpSumAlpha_) *eOneMinusExpSumAlpha_ += dynProgProb.getProb (value) *
(1.0 - exp (lambda_ * static_cast <double> (value)));
}
// check that not too much probability has been omitted
double prob = 0.0;
for (value = entry + 1; value < dynProgProb.getValueUpper (); value++) {
prob += dynProgProb.getProb (value);
}
prob += dynProgProb.getProbLost ();
#ifdef _DEBUG /* to eliminate compiler warning in release mode */
const double FUDGE = 2.0;
#endif
_ASSERT (prob <= FUDGE * static_cast <double> (dimension_) * REL_TOL);
}
/* >>> start tutorial code >>> */
int main( ) {
DifferentialState xT; // the trolley position
DifferentialState vT; // the trolley velocity
IntermediateState aT; // the trolley acceleration
DifferentialState xL; // the cable length
DifferentialState vL; // the cable velocity
IntermediateState aL; // the cable acceleration
DifferentialState phi; // the excitation angle
DifferentialState omega; // the angular velocity
DifferentialState uT; // trolley velocity control
DifferentialState uL; // cable velocity control
Control duT;
Control duL;
//
// DEFINE THE PARAMETERS:
//
const double tau1 = 0.012790605943772;
const double a1 = 0.047418203070092;
const double tau2 = 0.024695192379264;
const double a2 = 0.034087337273386;
const double g = 9.81;
const double c = 0.2;
const double m = 1318.0;
//
// DEFINE THE MODEL EQUATIONS:
//
DifferentialEquation f, f2, test_expr;
ExportAcadoFunction fun, fun2;
aT = -1.0 / tau1 * vT + a1 / tau1 * uT;
aL = -1.0 / tau2 * vL + a2 / tau2 * uL;
Expression states;
states << xT;
states << vT;
states << xL;
states << vL;
states << phi;
states << omega;
states << uT;
states << uL;
Expression controls;
controls << duT;
controls << duL;
Expression arg;
arg << states;
arg << controls;
int NX = states.getDim();
int NU = controls.getDim();
IntermediateState expr(2);
expr(0) = - 1.0/xL*(-g*sin(phi)-aT*cos(phi)-2*vL*omega-c*omega/(m*xL)) + log(duT/duL)*pow(xL,2);
expr(1) = - 1.0/xL*(-g*tan(phi)-aT*acos(phi)-2*atan(vL)*omega-c*asin(omega)/exp(xL)) + duT/pow(omega,3);
//~ expr(0) = - 1.0/xL*(-g*sin(phi));
//~ expr(1) = duT/pow(omega,3);
DifferentialState lambda("", expr.getDim(),1);
DifferentialState Sx("", states.getDim(),states.getDim());
DifferentialState Su("", states.getDim(),controls.getDim());
Expression S = Sx;
S.appendCols(Su);
// SYMMETRIC DERIVATIVES
Expression S_tmp = S;
S_tmp.appendRows(zeros<double>(NU,NX).appendCols(eye<double>(NU)));
IntermediateState dfS,dl;
Expression f_tmp = symmetricDerivative( expr, arg, S_tmp, lambda, &dfS, &dl );
f << returnLowerTriangular( f_tmp );
fun.init(f, "symmetricDerivative", NX*(1+NX+NU)+expr.getDim(), 0, 2, 0, 0, 0);
// ALTERNATIVE DERIVATIVES
IntermediateState tmp = backwardDerivative( expr, states, lambda );
IntermediateState tmp2 = forwardDerivative( tmp, states );
Expression tmp3 = backwardDerivative( expr, controls, lambda );
Expression tmp4 = multipleForwardDerivative( tmp3, states, Su );
Expression tmp5 = tmp4 + tmp4.transpose() + forwardDerivative( tmp3, controls );
Expression f2_tmp1;
f2_tmp1 = symmetricDoubleProduct( tmp2, Sx );
f2_tmp1.appendCols( zeros<double>(NX,NU) );
Expression f2_tmp2;
f2_tmp2 = Su.transpose()*tmp2*Sx + multipleForwardDerivative( tmp3, states, Sx );
f2_tmp2.appendCols( symmetricDoubleProduct( tmp2, Su ) + tmp5 );
f2_tmp1.appendRows( f2_tmp2 );
f2 << returnLowerTriangular( f2_tmp1 );
fun2.init(f2, "alternativeSymmetric", NX*(1+NX+NU)+expr.getDim(), 0, 2, 0, 0, 0);
Function f1;
f1 << dfS;
f1 << dl;
std::ofstream stream2( "ADtest/ADsymbolic_output2.c" );
stream2 << f1;
stream2.close();
std::ofstream stream( "ADtest/ADsymbolic_output.c" );
fun.exportCode( stream, "double" );
fun2.exportCode( stream, "double" );
test_expr << expr;
stream << test_expr;
stream.close();
return 0;
}
int CProblem::setModel()
{
//time_t start, end;
IloEnv env;
try
{
IloModel model(env);
IloCplex cplex(env);
/*Variables*/
IloNumVar lambda(env, "lambda");
IloNumVarArray c(env, n); //
for (unsigned int u = 0; u < n; u++)
{
std::stringstream ss;
ss << u;
std::string str = "c" + ss.str();
c[u] = IloNumVar(env, str.c_str());
}
IloArray<IloIntVarArray> z0(env,n);
for (unsigned int i=0; i<n; i++)
{
std::stringstream ss;
ss << i;
std::string str = "z0_" + ss.str();
z0[i]= IloIntVarArray(env, n, 0, 1);
}
/*
IloIntVarArray z0(env, Info.numAddVar);
for (int i = 0; i < Info.numAddVar; i++)
{
std::stringstream ss;
ss << i;
std::string str = "z0_" + ss.str();
z0[i] = IloIntVar(env, 0, 1, str.c_str());
}
*/
/* Objective*/
model.add(IloMinimize(env, lambda));
/*Constrains*/
/* d=function of the distance */
IloArray<IloNumArray> Par_d(env, n);
for (unsigned int u = 0; u < n; u++)
{
Par_d[u] = IloNumArray(env, n);
for (unsigned int v = 0; v < n; v++)
{
Par_d[u][v] = d[u][v];
}
}
int M = (max_distance + 1) * n;
/*
for (int i = 0; i < Info.numAddVar; i++)
{
int u = Info.ConstrIndex[i * 2];
int v = Info.ConstrIndex[i * 2 + 1];
model.add(c[u] - c[v] + M * (z0[i]) >= Par_d[u][v]);
model.add(c[v] - c[u] + M * (1 - z0[i]) >= Par_d[u][v]);
}
*/
for (unsigned u=0; u<n; u++)
{
for (unsigned v=0; v<u; v++)
{
if (Par_d[u][v] <= max_distance)
{
model.add(c[v]-c[u]+M*z0[u][v] >=Par_d[u][v]);
model.add(c[u]-c[v]+M*(1-z0[u][v]) >=Par_d[u][v]);
//Info.numvar++;
}
}
}
// d(x) = 3 - x
if (max_distance == 2)
{
// Sechsecke mit der Seitenlaenge 1
model.add(c[0]+c[2] +c[3] +c[5] +c[6] +c[9]>=16.6077);
model.add(c[1]+c[3] +c[4] +c[6] +c[7] +c[10]>=16.6077);
model.add(c[5]+c[8] +c[9] +c[12]+c[13]+c[16]>=16.6077);
model.add(c[6]+c[9] +c[10]+c[13]+c[14]+c[17]>=16.6077);
model.add(c[7]+c[10]+c[11]+c[14]+c[15]+c[18]>=16.6077);
model.add(c[13]+c[16]+c[17]+c[19]+c[20]+c[22]>=16.6077);
model.add(c[14]+c[17]+c[18]+c[20]+c[21]+c[23]>=16.6077);
}
//d(x) = 4 - x
if (max_distance == 3)
{
// Sechsecke mit der Seitenlaenge 1
model.add(c[0]+c[2] +c[3] +c[5] +c[6] +c[9]>=31.6077);
model.add(c[1]+c[3] +c[4] +c[6] +c[7] +c[10]>=31.6077);
model.add(c[5]+c[8] +c[9] +c[12]+c[13]+c[16]>=31.6077);
model.add(c[6]+c[9] +c[10]+c[13]+c[14]+c[17]>=31.6077);
model.add(c[7]+c[10]+c[11]+c[14]+c[15]+c[18]>=31.6077);
model.add(c[13]+c[16]+c[17]+c[19]+c[20]+c[22]>=31.6077);
model.add(c[14]+c[17]+c[18]+c[20]+c[21]+c[23]>=31.6077);
}
for (unsigned int v = 0; v < n; v++)
{
model.add(c[v] <= lambda);
model.add(c[v] >= 0);
}
std::cout << "Number of variables " << Info.numVar << "\n";
/* solve the Model*/
cplex.extract(model);
cplex.exportModel("L-Labeling.lp");
cplex.setParam(IloCplex::ClockType,1);
IloTimer timer(env);
timer.start();
int solveError = cplex.solve();
timer.stop();
if (!solveError)
{
std::cout << "STATUS : " << cplex.getStatus() << "\n";
env.error() << "Failed to optimize LP \n";
exit(1);
}
//Info.time = (double)(end-start)/(double)CLOCKS_PER_SEC;
Info.time = timer.getTime();
std::cout << "STATUS : " << cplex.getStatus() << "\n";
/* get the solution*/
env.out() << "Solution status = " << cplex.getStatus() << "\n";
Info.numConstr = cplex.getNrows();
env.out() << " Number of constraints = " << Info.numConstr << "\n";
lambda_G_d = cplex.getObjValue();
env.out() << "Solution value = " << lambda_G_d << "\n";
for (unsigned int u = 0; u < n; u++)
{
C.push_back(cplex.getValue(c[u]));
std::cout << "c(" << u << ")=" << C[u] << " ";
}
} // end try
catch (IloException& e)
{
std::cerr << "Concert exception caught: " << e << std::endl;
} catch (...)
{
std::cerr << "Unknown exception caught" << std::endl;
}
env.end();
return 0;
}
int main(int argc, const char* argv[])
{
std::string opt_ip = "131.254.10.126";
bool opt_language_english = true;
for (unsigned int i=0; i<argc; i++) {
if (std::string(argv[i]) == "--ip")
opt_ip = argv[i+1];
else if (std::string(argv[i]) == "--fr")
opt_language_english = false;
else if (std::string(argv[i]) == "--help") {
std::cout << "Usage: " << argv[0] << "[--ip <robot address>] [--fr]" << std::endl;
return 0;
}
}
// USE NEW MODULE
qi::SessionPtr session = qi::makeSession();
session->connect("tcp://131.254.10.126:9559");
qi::AnyObject proxy = session->service("pepper_control");
proxy.call<void >("start");
// -------------------------------------
std::string camera_name = "CameraTopPepper";
// Open the grabber for the acquisition of the images from the robot
vpNaoqiGrabber g;
if (! opt_ip.empty())
g.setRobotIp(opt_ip);
g.setFramerate(30);
g.setCamera(0);
g.open();
vpCameraParameters cam = vpNaoqiGrabber::getIntrinsicCameraParameters(AL::kQVGA,camera_name, vpCameraParameters::perspectiveProjWithDistortion);
vpHomogeneousMatrix eMc = vpNaoqiGrabber::getExtrinsicCameraParameters(camera_name,vpCameraParameters::perspectiveProjWithDistortion);
std::cout << "eMc:" << std::endl << eMc << std::endl;
// Connect to the robot
vpNaoqiRobot robot;
if (! opt_ip.empty())
robot.setRobotIp(opt_ip);
robot.open();
if (robot.getRobotType() != vpNaoqiRobot::Pepper)
{
std::cout << "ERROR: You are not connected to Pepper, but to a different Robot. Check the IP. " << std::endl;
return 0;
}
std::vector<std::string> jointNames_head = robot.getBodyNames("Head");
// Open Proxy for the speech
AL::ALTextToSpeechProxy tts(opt_ip, 9559);
std::string phraseToSay;
if (opt_language_english)
{
tts.setLanguage("English");
phraseToSay = " \\emph=2\\ Hi,\\pau=200\\ How are you ?";
}
else
{
tts.setLanguage("French");
phraseToSay = " \\emph=2\\ Bonjour,\\pau=200\\ comment vas tu ?";
}
// Plotting
vpPlot plotter_diff_vel (2);
plotter_diff_vel.initGraph(0, 2);
plotter_diff_vel.initGraph(1, 2);
plotter_diff_vel.setTitle(0, "HeadYaw");
plotter_diff_vel.setTitle(1, "HeadPitch");
vpPlot plotter_error (2);
plotter_error.initGraph(0, 1);
plotter_error.initGraph(1, 1);
plotter_error.setTitle(0, "HeadYaw");
plotter_error.setTitle(1, "HeadPitch");
try {
vpImage<unsigned char> I(g.getHeight(), g.getWidth());
vpDisplayX d(I);
vpDisplay::setTitle(I, "ViSP viewer");
vpFaceTrackerOkao face_tracker(opt_ip,9559);
// Initialize head servoing
vpServoHead servo_head;
servo_head.setCameraParameters(cam);
vpAdaptiveGain lambda(3.5, 0.8, 15); // lambda(0)=2, lambda(oo)=0.1 and lambda_dot(0)=10
servo_head.setLambda(lambda);
double servo_time_init = 0;
vpImagePoint head_cog_cur;
vpImagePoint head_cog_des(I.getHeight()/2, I.getWidth()/2);
vpColVector q_dot_head;
bool reinit_servo = true;
unsigned long loop_iter = 0;
std::vector<std::string> recognized_names;
std::map<std::string,unsigned int> detected_face_map;
bool detection_phase = true;
unsigned int f_count = 0;
double t_prev = vpTime::measureTimeSecond();
while(1) {
if (reinit_servo) {
servo_time_init = vpTime::measureTimeSecond();
t_prev = vpTime::measureTimeSecond();
reinit_servo = false;
//proxy.call<void >("start");
}
double t = vpTime::measureTimeMs();
g.acquire(I);
vpDisplay::display(I);
bool face_found = face_tracker.detect();
if (face_found) {
std::ostringstream text;
text << "Found " << face_tracker.getNbObjects() << " face(s)";
vpDisplay::displayText(I, 10, 10, text.str(), vpColor::red);
for(size_t i=0; i < face_tracker.getNbObjects(); i++) {
vpRect bbox = face_tracker.getBBox(i);
if (i == 0)
vpDisplay::displayRectangle(I, bbox, vpColor::red, false, 2);
else
vpDisplay::displayRectangle(I, bbox, vpColor::green, false, 1);
vpDisplay::displayText(I, (int)bbox.getTop()-10, (int)bbox.getLeft(), face_tracker.getMessage(i) , vpColor::red);
}
double u = face_tracker.getCog(0).get_u();
double v = face_tracker.getCog(0).get_v();
if (u<= g.getWidth() && v <= g.getHeight())
head_cog_cur.set_uv(u,v);
vpRect bbox = face_tracker.getBBox(0);
std::string name = face_tracker.getMessage(0);
// Get Head Jacobian (6x2)
vpMatrix eJe = robot.get_eJe("Head");
// Add column relative to the base rotation (Wz)
vpColVector col_wz(6);
col_wz[5] = 1;
eJe.resize(6,3,false);
for (unsigned int i =0; i <6; i++)
eJe[i][2] = col_wz[i];
// std::cout << "JAC" << std::endl << eJe << std::endl;
servo_head.set_eJe( eJe );
servo_head.set_cVe( vpVelocityTwistMatrix(eMc.inverse()) );
servo_head.setCurrentFeature(head_cog_cur);
servo_head.setDesiredFeature(head_cog_des);
//vpDisplay::setFont(I, "-*-*-bold-*-*-*-*-*-*-*-*-*-*-*");
//vpDisplay::displayText(I, face_tracker.getFace().getTopLeft()+vpImagePoint(-20,0), "Coraline", vpColor::red);
//vpServoDisplay::display(servo_head.m_task_head, cam, I, vpColor::green, vpColor::red, 3);
q_dot_head = servo_head.computeControlLaw(vpTime::measureTimeSecond() - servo_time_init);
std::vector<float> vel(jointNames_head.size());
for (unsigned int i=0 ; i < jointNames_head.size() ; i++) {
vel[i] = q_dot_head[i];
}
// Compute the distance in pixel between the target and the center of the image
double distance = vpImagePoint::distance(head_cog_cur, head_cog_des);
//if (distance > 0.03*I.getWidth())
std::cout << "vel" << std::endl << q_dot_head << std::endl;
proxy.async<void >("setDesJointVelocity", jointNames_head,vel );
robot.getProxy()->move(0.0, 0.0, q_dot_head[2]);
vpColVector vel_head = robot.getJointVelocity(jointNames_head);
for (unsigned int i=0 ; i < jointNames_head.size() ; i++) {
plotter_diff_vel.plot(i,1,loop_iter,q_dot_head[i]);
plotter_diff_vel.plot(i,0,loop_iter,vel_head[i]);
plotter_error.plot(i,0,loop_iter,servo_head.m_task_head.getError()[i]);
}
if (detection_phase)
{
//if (score >= 0.4 && distance < 0.06*I.getWidth() && bbox.getSize() > 3000)
if (distance < 0.06*I.getWidth() && bbox.getSize() > 3000)
{
vpDisplay::displayRectangle(I, bbox, vpColor::red, false, 1);
vpDisplay::displayText(I, (int)bbox.getTop()-10, (int)bbox.getLeft(), name, vpColor::red);
detected_face_map[name]++;
f_count++;
}
else
{
vpDisplay::displayRectangle(I, bbox, vpColor::green, false, 1);
vpDisplay::displayText(I, (int)bbox.getTop()-10, (int)bbox.getLeft(), name, vpColor::green);
}
if (f_count>10)
{
detection_phase = false;
f_count = 0;
}
}
else
{
std::string recognized_person_name = std::max_element(detected_face_map.begin(), detected_face_map.end(), pred)->first;
unsigned int times = std::max_element(detected_face_map.begin(), detected_face_map.end(), pred)->second;
if (!in_array(recognized_person_name, recognized_names) && recognized_person_name != "Unknown") {
if (opt_language_english)
{
phraseToSay = "\\emph=2\\ Hi \\wait=200\\ \\emph=2\\" + recognized_person_name + "\\pau=200\\ How are you ?";
}
else
{
phraseToSay = "\\emph=2\\ Salut \\wait=200\\ \\emph=2\\" + recognized_person_name + "\\pau=200\\ comment vas tu ?";;
}
std::cout << phraseToSay << std::endl;
tts.post.say(phraseToSay);
recognized_names.push_back(recognized_person_name);
}
if (!in_array(recognized_person_name, recognized_names) && recognized_person_name == "Unknown"
&& times > 15)
{
if (opt_language_english)
{
phraseToSay = "\\emph=2\\ Hi \\wait=200\\ \\emph=2\\. I don't know you! \\emph=2\\ What's your name?";
}
else
{
phraseToSay = " \\emph=2\\ Salut \\wait=200\\ \\emph=2\\. Je ne te connais pas! \\emph=2\\ Comment t'appelles-tu ?";
}
std::cout << phraseToSay << std::endl;
tts.post.say(phraseToSay);
recognized_names.push_back(recognized_person_name);
}
detection_phase = true;
detected_face_map.clear();
}
}
else {
proxy.call<void >("stopJoint");
robot.getProxy()->move(0.0, 0.0, 0.0);
std::cout << "Stop!" << std::endl;
reinit_servo = true;
}
vpDisplay::flush(I);
if (vpDisplay::getClick(I, false))
break;
loop_iter ++;
std::cout << "Loop time: " << vpTime::measureTimeMs() - t << " ms" << std::endl;
}
vpDisplay::getClick(I, true);
proxy.call<void >("stop");
robot.getProxy()->move(0.0, 0.0, 0.0);
}
catch(vpException &e) {
std::cout << e.getMessage() << std::endl;
}
catch (const AL::ALError& e)
{
std::cerr << "Caught exception " << e.what() << std::endl;
}
std::cout << "The end: stop the robot..." << std::endl;
proxy.call<void >("stop");
robot.getProxy()->move(0.0, 0.0, 0.0);
return 0;
}
void messageCallback() override { lambda (nullptr, error); }
/* ap (typ_ap(ap(f,x)), x) = typ (ap(f,x)) */
DEM typ_ap (DEM r)
{
DEM a, b, c, u, v, w, bz, cz, PIbzcz, bz1, tf, tf1, r1;
ORDER o, o1;
if (node(r) == _ap)
{
/*
if (subdem(1,r) == z && !in (z, subdem(0,r)))
return subdem(0,r);
a = typ (z);
u = typ (a);
*/
bz = typ (subdem(1,r));
/* b = fnc (bz); */
/* b = lambda (z, bz); */
/*PIbzcz = typ (subdem(0,r));
cz = arg (PIbzcz);*/
tf = typ (subdem(0,r));
new_tf:
if (node(tf) == _ap &&
node(subdem(0,tf)) == _ap)
{
if (node(subdem(0,subdem(0,tf))) == _F)
{
o = order(subdem(0,subdem(0,tf)));
bz1 = subdem(1,subdem(0,tf));
/* cz = Kottx (suc_order(o),*U(o)*/
cz = Kttx (typ(subdem(1,tf)), bz1, subdem(1,tf));
goto re;
}
else if (node(subdem(0,subdem(0,tf))) == _PI)
{
cz = arg (tf);
goto re;
}
}
/* r1 = ap (simplif (subdem(0,r)), subdem(1,r));
if (r1 != r) */
tf1 = simplif (tf);
if (tf1 != tf)
/*return lambda (z, r1);*/
{
tf = tf1;
goto new_tf;
}
/* error ("Bad type of function in lambda", __LINE__, tf); */
return NULL;
/* c = fnc (cz); */
/*c = lambda (z, cz);*/
re:
return cz;
#if 0
c = lambda (z, cz);
o1 = max_order (order_u(u), max_order (order_u(typ(bz)),
order_u(typ(tf)) ));
return Sotttxx (pred_order (o1/*order_u (u)*/), a, b, c,
lambda (z, subdem(0,r)),
lambda (z, subdem(1,r)));
#endif
}
else
{
/* error ("typ_ap : bad ap :", __LINE__, r); */
return NULL;
/*
a = typ (z);
b = typ (r);
u = typ (a);
return Kottx (pred_order (order_u(u)), b, a, r);
*/
}
}
int main(int argc, const char* argv[])
{
std::string opt_ip = "198.18.0.1";;
std::string opt_data_folder = std::string(ROMEOTK_DATA_FOLDER);
bool opt_Reye = false;
// Learning folder in /tmp/$USERNAME
std::string username;
// Get the user login name
vpIoTools::getUserName(username);
// Create a log filename to save new files...
std::string learning_folder;
#if defined(_WIN32)
learning_folder ="C:/temp/" + username;
#else
learning_folder ="/tmp/" + username;
#endif
// Test if the output path exist. If no try to create it
if (vpIoTools::checkDirectory(learning_folder) == false) {
try {
// Create the dirname
vpIoTools::makeDirectory(learning_folder);
}
catch (vpException &e) {
std::cout << "Cannot create " << learning_folder << std::endl;
std::cout << "Error: " << e.getMessage() <<std::endl;
return 0;
}
}
for (unsigned int i=0; i<argc; i++) {
if (std::string(argv[i]) == "--ip")
opt_ip = argv[i+1];
else if ( std::string(argv[i]) == "--reye")
opt_Reye = true;
else if (std::string(argv[i]) == "--help") {
std::cout << "Usage: " << argv[0] << "[--ip <robot address>]" << std::endl;
return 0;
}
}
std::vector<std::string> chain_name(2); // LArm or RArm
chain_name[0] = "LArm";
chain_name[1] = "RArm";
/************************************************************************************************/
/** Open the grabber for the acquisition of the images from the robot*/
vpNaoqiGrabber g;
g.setFramerate(15);
// Initialize constant transformations
vpHomogeneousMatrix eMc;
if (opt_Reye)
{
std::cout << "Using camera Eye Right" << std::endl;
g.setCamera(3); // CameraRightEye
eMc = g.get_eMc(vpCameraParameters::perspectiveProjWithDistortion,"CameraRightEye");
}
else
{
std::cout << "Using camera Eye Right" << std::endl;
g.setCamera(2); // CameraLeftEye
eMc = g.get_eMc(vpCameraParameters::perspectiveProjWithDistortion,"CameraLeftEye");
}
std::cout << "eMc: " << eMc << std::endl;
if (! opt_ip.empty())
g.setRobotIp(opt_ip);
g.open();
std::string camera_name = g.getCameraName();
std::cout << "Camera name: " << camera_name << std::endl;
vpCameraParameters cam = g.getCameraParameters(vpCameraParameters::perspectiveProjWithDistortion);
std::cout << "Camera parameters: " << cam << std::endl;
/** Initialization Visp Image, display and camera paramenters*/
vpImage<unsigned char> I(g.getHeight(), g.getWidth());
vpDisplayX d(I);
vpDisplay::setTitle(I, "Camera view");
//Initialize opencv color image
cv::Mat cvI = cv::Mat(cv::Size(g.getWidth(), g.getHeight()), CV_8UC3);
/************************************************************************************************/
/** Initialization target box*/
std::string opt_name_file_color_box_target = opt_data_folder + "/" +"target/plate_blob/color.txt";
vpBlobsTargetTracker box_tracker;
bool status_box_tracker;
vpHomogeneousMatrix cMbox;
const double L = 0.04/2;
std::vector <vpPoint> points(4);
points[2].setWorldCoordinates(-L,-L, 0) ;
points[1].setWorldCoordinates(-L,L, 0) ;
points[0].setWorldCoordinates(L,L, 0) ;
points[3].setWorldCoordinates(L,-L,0) ;
box_tracker.setName("plate");
box_tracker.setCameraParameters(cam);
box_tracker.setPoints(points);
box_tracker.setGrayLevelMaxBlob(60);
box_tracker.setLeftHandTarget(false);
if(!box_tracker.loadHSV(opt_name_file_color_box_target))
{
std::cout << "Error opening the file "<< opt_name_file_color_box_target << std::endl;
}
/************************************************************************************************/
/** Initialization target hands*/
std::string opt_name_file_color_target_path = opt_data_folder + "/" +"target/";
std::string opt_name_file_color_target_l = opt_name_file_color_target_path + chain_name[0] +"/color.txt";
std::string opt_name_file_color_target_r = opt_name_file_color_target_path + chain_name[1] +"/color.txt";
std::string opt_name_file_color_target1_l = opt_name_file_color_target_path + chain_name[0] +"/color1.txt";
std::string opt_name_file_color_target1_r = opt_name_file_color_target_path + chain_name[1] +"/color1.txt";
std::vector<vpBlobsTargetTracker*> hand_tracker;
std::vector<bool> status_hand_tracker(2);
std::vector<vpHomogeneousMatrix> cMo_hand(2);
const double L1 = 0.025/2;
std::vector <vpPoint> points1(4);
points1[2].setWorldCoordinates(-L1,-L1, 0) ;
points1[1].setWorldCoordinates(-L1,L1, 0) ;
points1[0].setWorldCoordinates(L1,L1, 0) ;
points1[3].setWorldCoordinates(L1,-L1,0) ;
vpBlobsTargetTracker hand_tracker_l;
hand_tracker_l.setName(chain_name[0]);
hand_tracker_l.setCameraParameters(cam);
hand_tracker_l.setPoints(points1);
hand_tracker_l.setLeftHandTarget(true);
if(!hand_tracker_l.loadHSV(opt_name_file_color_target_l))
std::cout << "Error opening the file "<< opt_name_file_color_target_l << std::endl;
vpBlobsTargetTracker hand_tracker_r;
hand_tracker_r.setName(chain_name[1]);
hand_tracker_r.setCameraParameters(cam);
hand_tracker_r.setPoints(points);
hand_tracker_r.setLeftHandTarget(false);
if(!hand_tracker_r.loadHSV(opt_name_file_color_target1_r))
std::cout << "Error opening the file "<< opt_name_file_color_target_r << std::endl;
hand_tracker.push_back(&hand_tracker_l);
hand_tracker.push_back(&hand_tracker_r);
/************************************************************************************************/
// Constant transformation Target Frame to Arm end-effector (WristPitch)
std::vector<vpHomogeneousMatrix> hand_Me_Arm(2);
std::string filename_transform = std::string(ROMEOTK_DATA_FOLDER) + "/transformation.xml";
// Create twist matrix from box to Arm end-effector (WristPitch)
std::vector <vpVelocityTwistMatrix> box_Ve_Arm(2);
// Create twist matrix from target Frame to Arm end-effector (WristPitch)
std::vector <vpVelocityTwistMatrix> hand_Ve_Arm(2);
// Constant transformation Target Frame to box to target Hand
std::vector<vpHomogeneousMatrix> box_Mhand(2);
for (unsigned int i = 0; i < 2; i++)
{
std::string name_transform = "qrcode_M_e_" + chain_name[i];
vpXmlParserHomogeneousMatrix pm; // Create a XML parser
if (pm.parse(hand_Me_Arm[i], filename_transform, name_transform) != vpXmlParserHomogeneousMatrix::SEQUENCE_OK) {
std::cout << "Cannot found the homogeneous matrix named " << name_transform << "." << std::endl;
return 0;
}
else
std::cout << "Homogeneous matrix " << name_transform <<": " << std::endl << hand_Me_Arm[i] << std::endl;
hand_Ve_Arm[i].buildFrom(hand_Me_Arm[i]);
}
/************************************************************************************************/
/** Create a new istance NaoqiRobot*/
vpNaoqiRobot robot;
if (! opt_ip.empty())
robot.setRobotIp(opt_ip);
robot.open();
std::vector<std::string> jointNames = robot.getBodyNames("Head");
//jointNames.pop_back(); // We don't consider the last joint of the head = HeadRoll
std::vector<std::string> jointNamesLEye = robot.getBodyNames("LEye");
std::vector<std::string> jointNamesREye = robot.getBodyNames("REye");
jointNames.insert(jointNames.end(), jointNamesLEye.begin(), jointNamesLEye.end());
std::vector<std::string> jointHeadNames_tot = jointNames;
jointHeadNames_tot.push_back(jointNamesREye.at(0));
jointHeadNames_tot.push_back(jointNamesREye.at(1));
/************************************************************************************************/
std::vector<bool> grasp_servo_converged(2);
grasp_servo_converged[0]= false;
grasp_servo_converged[1]= false;
std::vector<vpMatrix> eJe(2);
// Initialize arms servoing
vpServoArm servo_larm_l;
vpServoArm servo_larm_r;
std::vector<vpServoArm*> servo_larm;
servo_larm.push_back(&servo_larm_l);
servo_larm.push_back(&servo_larm_r);
std::vector < std::vector<std::string > > jointNames_arm(2);
jointNames_arm[0] = robot.getBodyNames(chain_name[0]);
jointNames_arm[1] = robot.getBodyNames(chain_name[1]);
// Delete last joint Hand, that we don't consider in the servo
jointNames_arm[0].pop_back();
jointNames_arm[1].pop_back();
std::vector<std::string> jointArmsNames_tot = jointNames_arm[0];
jointArmsNames_tot.insert(jointArmsNames_tot.end(), jointNames_arm[1].begin(), jointNames_arm[1].end());
const unsigned int numArmJoints = jointNames_arm[0].size();
std::vector<vpHomogeneousMatrix> box_dMbox(2);
// Vector containing the joint velocity vectors of the arms
std::vector<vpColVector> q_dot_arm;
// Vector containing the joint velocity vectors of the arms for the secondary task
std::vector<vpColVector> q_dot_arm_sec;
// Vector joint position of the arms
std::vector<vpColVector> q;
// Vector joint real velocities of the arms
std::vector<vpColVector> q_dot_real;
vpColVector q_temp(numArmJoints);
q_dot_arm.push_back(q_temp);
q_dot_arm.push_back(q_temp);
q_dot_arm_sec.push_back(q_temp);
q_dot_arm_sec.push_back(q_temp);
q.push_back(q_temp);
q.push_back(q_temp);
q_dot_real.push_back(q_temp);
q_dot_real.push_back(q_temp);
// Initialize the joint avoidance scheme from the joint limits
std::vector<vpColVector> jointMin;
std::vector<vpColVector> jointMax;
jointMin.push_back(q_temp);
jointMin.push_back(q_temp);
jointMax.push_back(q_temp);
jointMax.push_back(q_temp);
for (unsigned int i = 0; i< 2; i++)
{
jointMin[i] = robot.getJointMin(chain_name[i]);
jointMax[i] = robot.getJointMax(chain_name[i]);
jointMin[i].resize(numArmJoints,false);
jointMax[i].resize(numArmJoints,false);
// std::cout << jointMin[i].size() << std::endl;
// std::cout << jointMax[i].size() << std::endl;
// std::cout << "max " << jointMax[i] << std::endl;
}
std::vector<bool> first_time_arm_servo(2);
first_time_arm_servo[0] = true;
first_time_arm_servo[1] = true;
std::vector< double> servo_arm_time_init(2);
servo_arm_time_init[0] = 0.0;
servo_arm_time_init[1] = 0.0;
std::vector<int> cpt_iter_servo_grasp(2);
cpt_iter_servo_grasp[0] = 0;
cpt_iter_servo_grasp[1] = 0;
unsigned int index_hand = 1;
// vpHomogeneousMatrix M_offset;
// M_offset[1][3] = 0.00;
// M_offset[0][3] = 0.00;
// M_offset[2][3] = 0.00;
vpHomogeneousMatrix M_offset;
M_offset.buildFrom(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) ;
double d_t = 0.01;
double d_r = 0.0;
bool first_time_box_pose = true;
/************************************************************************************************/
vpMouseButton::vpMouseButtonType button;
vpHomogeneousMatrix elMb; // Homogeneous matrix from right wrist roll to box
vpHomogeneousMatrix cMbox_d; // Desired box final pose
State_t state;
state = CalibrateRigthArm;
//AL::ALValue names_head = AL::ALValue::array("NeckYaw","NeckPitch","HeadPitch","HeadRoll","LEyeYaw", "LEyePitch","REyeYaw", "REyePitch" );
// Big Plate
// AL::ALValue angles_head = AL::ALValue::array(vpMath::rad(-23.2), vpMath::rad(16.6), vpMath::rad(10.3), vpMath::rad(0.0), 0.0 , 0.0, 0.0, 0.0 );
// small plate
AL::ALValue angles_head = AL::ALValue::array(vpMath::rad(-15.2), vpMath::rad(17.6), vpMath::rad(10.3), vpMath::rad(0.0), 0.0 , vpMath::rad(9.8), 0.0, 0.0 );
float fractionMaxSpeed = 0.1f;
robot.getProxy()->setAngles(jointHeadNames_tot, angles_head, fractionMaxSpeed);
while(1) {
double loop_time_start = vpTime::measureTimeMs();
g.acquire(cvI);
vpImageConvert::convert(cvI, I);
vpDisplay::display(I);
bool click_done = vpDisplay::getClick(I, button, false);
// if (state < VSBox)
// {
char key[10];
bool ret = vpDisplay::getKeyboardEvent(I, key, false);
std::string s = key;
if (ret)
{
if (s == "r")
{
M_offset.buildFrom(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) ;
d_t = 0.0;
d_r = 0.2;
std::cout << "Rotation mode. " << std::endl;
}
if (s == "t")
{
M_offset.buildFrom(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) ;
d_t = 0.01;
d_r = 0.0;
std::cout << "Translation mode. " << std::endl;
}
if (s == "h")
{
hand_tracker[index_hand]->setManualBlobInit(true);
hand_tracker[index_hand]->setForceDetection(true);
}
else if ( s == "+")
{
unsigned int value = hand_tracker[index_hand]->getGrayLevelMaxBlob() +10;
hand_tracker[index_hand]->setGrayLevelMaxBlob(value);
std::cout << "Set to "<< value << "the value of " << std::endl;
}
else if (s == "-")
{
unsigned int value = hand_tracker[1]->getGrayLevelMaxBlob()-10;
hand_tracker[index_hand]->setGrayLevelMaxBlob(value-10);
std::cout << "Set to "<< value << " GrayLevelMaxBlob. " << std::endl;
}
// |x
// z\ |
// \ |
// \|_____ y
//
else if (s == "4") //-y
{
M_offset.buildFrom(0.0, -d_t, 0.0, 0.0, -d_r, 0.0) ;
}
else if (s == "6") //+y
{
M_offset.buildFrom(0.0, d_t, 0.0, 0.0, d_r, 0.0) ;
}
else if (s == "8") //+x
{
M_offset.buildFrom(d_t, 0.0, 0.0, d_r, 0.0, 0.0) ;
}
else if (s == "2") //-x
{
M_offset.buildFrom(-d_t, 0.0, 0.0, -d_r, 0.0, 0.0) ;
}
else if (s == "7")//-z
{
M_offset.buildFrom(0.0, 0.0, -d_t, 0.0, 0.0, -d_r) ;
}
else if (s == "9") //+z
{
M_offset.buildFrom(0.0, 0.0, d_t, 0.0, 0.0, d_r) ;
}
cMbox_d = cMbox_d * M_offset;
}
if (state < WaitPreGrasp)
{
status_hand_tracker[index_hand] = hand_tracker[index_hand]->track(cvI,I);
if (status_hand_tracker[index_hand] ) { // display the tracking results
cMo_hand[index_hand] = hand_tracker[index_hand]->get_cMo();
printPose("cMo right arm: ", cMo_hand[index_hand]);
// The qrcode frame is only displayed when PBVS is active or learning
vpDisplay::displayFrame(I, cMo_hand[index_hand], cam, 0.04, vpColor::none, 3);
}
}
// }
status_box_tracker = box_tracker.track(cvI,I);
if (status_box_tracker ) { // display the tracking results
cMbox = box_tracker.get_cMo();
printPose("cMo box: ", cMbox);
// The qrcode frame is only displayed when PBVS is active or learning
vpDisplay::displayFrame(I, cMbox, cam, 0.04, vpColor::none, 1);
// if (first_time_box_pose)
// {
// // Compute desired box position cMbox_d
// cMbox_d = cMbox * M_offset;
// first_time_box_pose = false;
// }
// vpDisplay::displayFrame(I, cMbox_d, cam, 0.04, vpColor::red, 1);
}
if (state == CalibrateRigthArm && status_box_tracker && status_hand_tracker[index_hand] )
{
vpDisplay::displayText(I, vpImagePoint(I.getHeight() - 10, 10), "Left click to calibrate right hand", vpColor::red);
vpHomogeneousMatrix box_Me_Arm; // Homogeneous matrix from the box to the left wrist
box_Mhand[index_hand] = cMbox.inverse() * cMo_hand[index_hand];
box_Me_Arm = box_Mhand[index_hand] * hand_Me_Arm[index_hand] ; // from box to WristPitch
// vpDisplay::displayFrame(I, cMo_hand[index_hand] * (cMbox.inverse() * cMo_hand[index_hand]).inverse() , cam, 0.04, vpColor::green, 1);
if (click_done && button == vpMouseButton::button1 ) {
box_Ve_Arm[index_hand].buildFrom(box_Me_Arm);
index_hand = 0;
state = CalibrateLeftArm;
// BIG plate
//AL::ALValue names_head = AL::ALValue::array("NeckYaw","NeckPitch","HeadPitch","HeadRoll","LEyeYaw", "LEyePitch","LEyeYaw", "LEyePitch" );
// AL::ALValue angles_head = AL::ALValue::array(vpMath::rad(8.7), vpMath::rad(16.6), vpMath::rad(10.3), vpMath::rad(0.0), 0.0 , 0.0, 0.0, 0.0 );
// Small Plate
AL::ALValue angles_head = AL::ALValue::array(vpMath::rad(2.4), vpMath::rad(17.6), vpMath::rad(10.3), vpMath::rad(0.0), 0.0 , vpMath::rad(9.8), 0.0, 0.0 );
float fractionMaxSpeed = 0.1f;
robot.getProxy()->setAngles(jointHeadNames_tot, angles_head, fractionMaxSpeed);
click_done = false;
}
}
if (state == CalibrateLeftArm && status_box_tracker && status_hand_tracker[index_hand] )
{
vpDisplay::displayText(I, vpImagePoint(I.getHeight() - 10, 10), "Left click to calibrate left hand", vpColor::red);
vpHomogeneousMatrix box_Me_Arm; // Homogeneous matrix from the box to the left wrist
box_Mhand[index_hand] = cMbox.inverse() * cMo_hand[index_hand];
box_Me_Arm = box_Mhand[index_hand] * hand_Me_Arm[index_hand] ; // from box to WristPitch
// if (first_time_box_pose)
// {
// // Compute desired box position cMbox_d
// cMbox_d = cMbox * M_offset;
// first_time_box_pose = false;
// }
// vpDisplay::displayFrame(I, cMbox_d, cam, 0.04, vpColor::red, 1);
// vpDisplay::displayFrame(I, cMo_hand[index_hand] * (cMbox.inverse() * cMo_hand[index_hand]).inverse() , cam, 0.04, vpColor::green, 1);
if (click_done && button == vpMouseButton::button1 ) {
box_Ve_Arm[index_hand].buildFrom(box_Me_Arm);
state = WaitPreGrasp;
click_done = false;
//AL::ALValue names_head = AL::ALValue::array("NeckYaw","NeckPitch","HeadPitch","HeadRoll","LEyeYaw", "LEyePitch","LEyeYaw", "LEyePitch" );
AL::ALValue angles_head = AL::ALValue::array(vpMath::rad(-6.8), vpMath::rad(16.6), vpMath::rad(10.3), vpMath::rad(0.0), 0.0 , 0.0, 0.0, 0.0 );
float fractionMaxSpeed = 0.05f;
robot.getProxy()->setAngles(jointHeadNames_tot, angles_head, fractionMaxSpeed);
}
}
if (state == WaitPreGrasp )
{
index_hand = 1;
if (click_done && button == vpMouseButton::button1 ) {
state = PreGraps;
click_done = false;
}
}
if (state == PreGraps && status_box_tracker )
{
vpDisplay::displayText(I, vpImagePoint(I.getHeight() - 10, 10), "Left click to start the servo", vpColor::red);
if (first_time_box_pose)
{
// Compute desired box position cMbox_d
cMbox_d = cMbox * M_offset;
first_time_box_pose = false;
}
vpDisplay::displayFrame(I, cMbox_d , cam, 0.05, vpColor::none, 3);
vpDisplay::displayFrame(I, cMbox *box_Mhand[1] , cam, 0.05, vpColor::none, 3);
vpDisplay::displayFrame(I, cMbox *box_Mhand[0] , cam, 0.05, vpColor::none, 3);
if (click_done && button == vpMouseButton::button1 ) {
state = VSBox;
click_done = false;
hand_tracker[index_hand] = &box_tracker; // Trick to use keys
}
}
if (state == VSBox)
{
//Get Actual position of the arm joints
q[0] = robot.getPosition(jointNames_arm[0]);
q[1] = robot.getPosition(jointNames_arm[1]);
// if(status_box_tracker && status_hand_tracker[index_hand] )
if(status_box_tracker )
{
if (! grasp_servo_converged[0]) {
// for (unsigned int i = 0; i<2; i++)
// {
unsigned int i = 0;
//vpAdaptiveGain lambda(0.8, 0.05, 8);
vpAdaptiveGain lambda(0.4, 0.02, 4);
//servo_larm[i]->setLambda(lambda);
servo_larm[i]->setLambda(0.2);
eJe[i] = robot.get_eJe(chain_name[i]);
servo_larm[i]->set_eJe(eJe[i]);
servo_larm[i]->m_task.set_cVe(box_Ve_Arm[i]);
box_dMbox[i] = cMbox_d.inverse() * cMbox;
printPose("box_dMbox: ", box_dMbox[i]);
servo_larm[i]->setCurrentFeature(box_dMbox[i]) ;
vpDisplay::displayFrame(I, cMbox_d , cam, 0.05, vpColor::none, 3);
// vpDisplay::displayFrame(I, cMbox *box_Mhand[1] , cam, 0.05, vpColor::none, 3);
// vpDisplay::displayFrame(I, cMbox *box_Mhand[0] , cam, 0.05, vpColor::none, 3);
if (first_time_arm_servo[i]) {
std::cout << "-- Start visual servoing of the arm" << chain_name[i] << "." << std::endl;
servo_arm_time_init[i] = vpTime::measureTimeSecond();
first_time_arm_servo[i] = false;
}
q_dot_arm[i] = - servo_larm[i]->computeControlLaw(servo_arm_time_init[i]);
q_dot_real[0] = robot.getJointVelocity(jointNames_arm[0]);
q_dot_real[1] = robot.getJointVelocity(jointNames_arm[1]);
// std::cout << "real_q: " << std::endl << real_q << std::endl;
// std::cout << " box_Ve_Arm[i]: " << std::endl << box_Ve_Arm[i] << std::endl;
// std::cout << " eJe[i][i]: " << std::endl << eJe[i] << std::endl;
//vpColVector real_v = (box_Ve_Arm[i] * eJe[i]) * q_dot_real[0];
vpColVector real_v = (box_Ve_Arm[i] * eJe[i]) * q_dot_arm[0];
// std::cout << "real_v: " << std::endl <<real_v << std::endl;
// vpVelocityTwistMatrix cVo(cMo_hand);
// vpMatrix cJe = cVo * oJo;
// Compute the feed-forward terms
// vpColVector sec_ter = 0.5 * ((servo_head.m_task_head.getTaskJacobianPseudoInverse() * (servo_head.m_task_head.getInteractionMatrix() * cJe)) * q_dot_larm);
// std::cout <<"Second Term:" <<sec_ter << std::endl;
//q_dot_head = q_dot_head + sec_ter;
// Compute joint limit avoidance
q_dot_arm_sec[0] = servo_larm[0]->m_task.secondaryTaskJointLimitAvoidance(q[0], q_dot_real[0], jointMin[0], jointMax[0]);
//q_dot_arm_sec[1] = servo_larm[1]->m_task.secondaryTaskJointLimitAvoidance(q[1], q_dot_real[1], jointMin[1], jointMax[1]);
// vpColVector q_dot_arm_head = q_dot_larm + q2_dot;
//q_dot_arm_head.stack(q_dot_tot);
// robot.setVelocity(joint_names_arm_head,q_dot_arm_head);
//robot.setVelocity(jointNames_arm[i], q_dot_larm);
// if (opt_plotter_arm) {
// plotter_arm->plot(0, cpt_iter_servo_grasp, servo_larm.m_task.getError());
// plotter_arm->plot(1, cpt_iter_servo_grasp, q_dot_larm);
// }
// if (opt_plotter_q_sec_arm)
// {
// plotter_q_sec_arm->plot(0,loop_iter,q2_dot);
// plotter_q_sec_arm->plot(1,loop_iter,q_dot_larm + q2_dot);
// }
cpt_iter_servo_grasp[i] ++;
// }
// // Visual servoing slave
// i = 1;
// //vpAdaptiveGain lambda(0.4, 0.02, 4);
// servo_larm[i]->setLambda(0.07);
// servo_larm[i]->set_eJe(robot.get_eJe(chain_name[i]));
// servo_larm[i]->m_task.set_cVe(hand_Ve_Arm[i]);
// box_dMbox[i] = (cMbox *box_Mhand[1]).inverse() * cMo_hand[1] ;
// printPose("box_dMbox: ", box_dMbox[i]);
// servo_larm[i]->setCurrentFeature(box_dMbox[i]) ;
// vpDisplay::displayFrame(I, cMbox_d , cam, 0.025, vpColor::red, 2);
// if (first_time_arm_servo[i]) {
// std::cout << "-- Start visual servoing of the arm" << chain_name[i] << "." << std::endl;
// servo_arm_time_init[i] = vpTime::measureTimeSecond();
// first_time_arm_servo[i] = false;
// }
// q_dot_arm[i] = - servo_larm[i]->computeControlLaw(servo_arm_time_init[i]);
eJe[1] = robot.get_eJe(chain_name[1]);
// q_dot_arm[1] += (box_Ve_Arm[1] * eJe[1]).pseudoInverse() * real_v;
q_dot_arm[1] = (box_Ve_Arm[1] * eJe[1]).pseudoInverse() * real_v;
vpColVector q_dot_tot = q_dot_arm[0] + q_dot_arm_sec[0];
q_dot_tot.stack( q_dot_arm[1] + q_dot_arm_sec[1]);
robot.setVelocity(jointArmsNames_tot, q_dot_tot);
vpTranslationVector t_error_grasp = box_dMbox[0].getTranslationVector();
vpRotationMatrix R_error_grasp;
box_dMbox[0].extract(R_error_grasp);
vpThetaUVector tu_error_grasp;
tu_error_grasp.buildFrom(R_error_grasp);
double theta_error_grasp;
vpColVector u_error_grasp;
tu_error_grasp.extract(theta_error_grasp, u_error_grasp);
std::cout << "error: " << t_error_grasp << " " << vpMath::deg(theta_error_grasp) << std::endl;
// if (cpt_iter_servo_grasp[0] > 100) {
// vpDisplay::displayText(I, vpImagePoint(10,10), "Cannot converge. Click to continue", vpColor::red);
// }
// double error_t_treshold = 0.007;
// if ( (sqrt(t_error_grasp.sumSquare()) < error_t_treshold) && (theta_error_grasp < vpMath::rad(3)) || (click_done && button == vpMouseButton::button1 /*&& cpt_iter_servo_grasp > 150*/) )
// {
// robot.stop(jointArmsNames_tot);
// state = End;
// grasp_servo_converged[0] = true;
// if (click_done && button == vpMouseButton::button1)
// click_done = false;
// }
}
}
else {
// state grasping but one of the tracker fails
robot.stop(jointArmsNames_tot);
}
}
if (state == End)
{
std::cout<< "End" << std::endl;
}
vpDisplay::flush(I) ;
if (click_done && button == vpMouseButton::button3) { // Quit the loop
break;
}
//std::cout << "Loop time: " << vpTime::measureTimeMs() - loop_time_start << std::endl;
}
robot.stop(jointArmsNames_tot);
}
int main(int argc, char* argv[])
{
// Load the mesh.
MeshSharedPtr mesh(new Mesh);
MeshReaderH2D mloader;
mloader.load("square.mesh", mesh);
// Perform initial mesh refinements.
for(int i = 0; i < INIT_GLOB_REF_NUM; i++)
mesh->refine_all_elements();
mesh->refine_towards_boundary("Bdy", INIT_BDY_REF_NUM);
// Initialize boundary conditions.
CustomEssentialBCNonConst bc_essential("Bdy");
EssentialBCs<double> bcs(&bc_essential);
// Create an H1 space with default shapeset.
SpaceSharedPtr<double> space(new H1Space<double>(mesh, &bcs, P_INIT));
int ndof = space->get_num_dofs();
// Initialize previous iteration solution for the Picard's method.
MeshFunctionSharedPtr<double> init_condition(new ConstantSolution<double>(mesh, INIT_COND_CONST));
// Initialize the weak formulation.
CustomNonlinearity lambda(alpha);
Hermes2DFunction<double> src(-heat_src);
CustomWeakFormPicard wf(init_condition, &lambda, &src);
// Initialize the Picard solver.
PicardSolver<double> picard(&wf, space);
picard.set_max_allowed_iterations(PICARD_MAX_ITER);
logger.info("Default tolerance");
try
{
picard.solve(init_condition);
}
catch(std::exception& e)
{
std::cout << e.what();
}
int iter_1 = picard.get_num_iters();
logger.info("Adjusted tolerance without Anderson");
// Perform the Picard's iteration (Anderson acceleration on by default).
picard.clear_tolerances();
picard.set_tolerance(PICARD_TOL, Hermes::Solvers::SolutionChangeAbsolute);
try
{
picard.solve(init_condition);
}
catch(std::exception& e)
{
std::cout << e.what();
}
int iter_2 = picard.get_num_iters();
// Translate the coefficient vector into a Solution.
MeshFunctionSharedPtr<double> sln(new Solution<double>);
Solution<double>::vector_to_solution(picard.get_sln_vector(), space, sln);
logger.info("Default tolerance without Anderson and good initial guess");
PicardSolver<double> picard2(&wf, space);
picard2.set_tolerance(1e-3, Hermes::Solvers::SolutionChangeRelative);
picard2.set_max_allowed_iterations(PICARD_MAX_ITER);
try
{
picard2.solve(sln);
}
catch(std::exception& e)
{
std::cout << e.what();
}
int iter_3 = picard2.get_num_iters();
logger.info("Default tolerance with Anderson and good initial guess");
picard2.use_Anderson_acceleration(true);
picard2.set_num_last_vector_used(PICARD_NUM_LAST_ITER_USED);
picard2.set_anderson_beta(PICARD_ANDERSON_BETA);
try
{
picard2.solve(sln);
}
catch(std::exception& e)
{
std::cout << e.what();
}
int iter_4 = picard2.get_num_iters();
#ifdef SHOW_OUTPUT
// Visualise the solution and mesh.
ScalarView s_view("Solution", new WinGeom(0, 0, 440, 350));
s_view.show_mesh(false);
s_view.show(sln);
OrderView o_view("Mesh", new WinGeom(450, 0, 420, 350));
o_view.show(space);
// Wait for all views to be closed.
View::wait();
#endif
bool success = Testing::test_value(iter_1, 14, "# of iterations 1", 1); // Tested value as of September 2013.
success = Testing::test_value(iter_2, 12, "# of iterations 2", 1) & success; // Tested value as of September 2013.
success = Testing::test_value(iter_3, 3, "# of iterations 3", 1) & success; // Tested value as of September 2013.
success = Testing::test_value(iter_4, 3, "# of iterations 4", 1) & success; // Tested value as of September 2013.
if(success)
{
printf("Success!\n");
return 0;
}
else
{
printf("Failure!\n");
return -1;
}
}
void Foam::MULES::limitSum(UPtrList<scalargpuField>& phiPsiCorrs)
{
label size = phiPsiCorrs[0].size();
scalargpuField sumPos(size,0);
scalargpuField sumNeg(size,0);
scalargpuField lambda(size,0);
boolgpuList positive(size,false);
boolgpuList negative(size,false);
for (int phasei=0; phasei<phiPsiCorrs.size(); phasei++)
{
thrust::transform
(
phiPsiCorrs[phasei].begin(),
phiPsiCorrs[phasei].end(),
thrust::make_zip_iterator(thrust::make_tuple
(
sumPos.begin(),
sumNeg.begin()
)),
thrust::make_zip_iterator(thrust::make_tuple
(
sumPos.begin(),
sumNeg.begin()
)),
sumPosSumNegMULESFunctor()
);
}
thrust::transform
(
sumPos.begin(),
sumPos.end(),
sumNeg.begin(),
thrust::make_zip_iterator(thrust::make_tuple
(
lambda.begin(),
positive.begin(),
negative.begin()
)),
lambdaMULESFunctor()
);
for (int phasei=0; phasei<phiPsiCorrs.size(); phasei++)
{
thrust::transform
(
phiPsiCorrs[phasei].begin(),
phiPsiCorrs[phasei].end(),
thrust::make_zip_iterator(thrust::make_tuple
(
lambda.begin(),
positive.begin(),
negative.begin()
)),
phiPsiCorrs[phasei].begin(),
phiPsiCorrsMULESFunctor()
);
}
}
void OSNSPTest::testAVI()
{
std::cout << "===========================================" <<std::endl;
std::cout << " ===== OSNSP tests start ... ===== " <<std::endl;
std::cout << "===========================================" <<std::endl;
std::cout << "------- implicit Twisting relation -------" <<std::endl;
_h = 1e-1;
_T = 20.0;
double G = 10.0;
double beta = .3;
_A->zero();
(*_A)(0, 1) = 1.0;
_x0->zero();
(*_x0)(0) = 10.0;
(*_x0)(1) = 10.0;
SP::SimpleMatrix B(new SimpleMatrix(_n, _n, 0));
SP::SimpleMatrix C(new SimpleMatrix(_n, _n));
(*B)(1, 0) = G;
(*B)(1, 1) = G*beta;
C->eye();
SP::FirstOrderLinearTIR rel(new FirstOrderLinearTIR(C, B));
SP::SimpleMatrix H(new SimpleMatrix(4, 2));
(*H)(0, 0) = 1.0;
(*H)(1, 0) = -_h/2.0;
(*H)(2, 0) = -1.0;
(*H)(3, 0) = _h/2.0;
(*H)(1, 1) = 1.0;
(*H)(3, 1) = -1.0;
SP::SiconosVector K(new SiconosVector(4));
(*K)(0) = -1.0;
(*K)(1) = -1.0;
(*K)(2) = -1.0;
(*K)(3) = -1.0;
SP::NonSmoothLaw nslaw(new NormalConeNSL(_n, H, K));
_DS.reset(new FirstOrderLinearTIDS(_x0, _A, _b));
_TD.reset(new TimeDiscretisation(_t0, _h));
_model.reset(new Model(_t0, _T));
SP::Interaction inter(new Interaction(_n, nslaw, rel));
_osi.reset(new EulerMoreauOSI(_theta));
_model->nonSmoothDynamicalSystem()->insertDynamicalSystem(_DS);
_model->nonSmoothDynamicalSystem()->setOSI(_DS, _osi);
_model->nonSmoothDynamicalSystem()->link(inter, _DS);
_sim.reset(new TimeStepping(_TD));
_sim->insertIntegrator(_osi);
SP::AVI osnspb(new AVI());
_sim->insertNonSmoothProblem(osnspb);
_model->initialize(_sim);
SimpleMatrix dataPlot((unsigned)ceil((_T - _t0) / _h) + 10, 5);
SiconosVector& xProc = *_DS->x();
SiconosVector& lambda = *inter->lambda(0);
unsigned int k = 0;
dataPlot(0, 0) = _t0;
dataPlot(0, 1) = (*_x0)(0);
dataPlot(0, 2) = (*_x0)(1);
dataPlot(0, 3) = -1.0;
dataPlot(0, 4) = -1.0;
while (_sim->hasNextEvent())
{
_sim->computeOneStep();
k++;
dataPlot(k, 0) = _sim->nextTime();
dataPlot(k, 1) = xProc(0);
dataPlot(k, 2) = xProc(1);
dataPlot(k, 3) = lambda(0);
dataPlot(k, 4) = lambda(1);
_sim->nextStep();
}
std::cout <<std::endl <<std::endl;
dataPlot.resize(k, dataPlot.size(1));
ioMatrix::write("testAVI.dat", "ascii", dataPlot, "noDim");
// Reference Matrix
SimpleMatrix dataPlotRef(dataPlot);
dataPlotRef.zero();
ioMatrix::read("testAVI.ref", "ascii", dataPlotRef);
std::cout << "------- Integration Ok, error = " << (dataPlot - dataPlotRef).normInf() << " -------" <<std::endl;
CPPUNIT_ASSERT_EQUAL_MESSAGE("testAVI : ", (dataPlot - dataPlotRef).normInf() < _tol, true);
}
void refineRowVNegative(double *mPostU, double *vPostU, double *mPostV,
double *vPostV, double *vPriorVj, double *vPriorVh, double *mPriorV,
double *mPred, double *vPred, int i, int j, int T, int P,
int k, double *e0, int *nOnesSampling, int *nZerosSampling,
int *nOnesAuxj, int *nZerosAuxj, double rho) {
int h;
double eta1New, eta2New, eta1Old, eta2Old, eta1Prior, eta2Prior,
eta1Post, eta2Post, zeta, reps, vNew, e;
/* We update the posterior approx. for the i-th row of U */
for (h = 0 ; h < k ; h++) {
zeta = sqrt((*mPred) * (*mPred) + *vPred);
e = logistic((*mPred) + log(*e0 / (1 - *e0)));
*mPred -= mPostU[ i + h * T ] * mPostV[ j + h * P ];
*vPred -= mPostU[ i + h * T ] * mPostU[ i + h * T ] *
vPostV[ j + h * P ] + vPostU[ i + h * T ] *
mPostV[ j + h * P ] * mPostV[ j + h * P ] +
vPostU[ i + h * T ] * vPostV[ j + h * P ];
eta2Prior = 1 / (vPriorVj[ j ] * vPriorVh[ h ]);
eta1Prior = mPriorV[ h ] / (vPriorVj[ j ] * vPriorVh[ h ]);
eta2Post = 1 / vPostV[ j + h * P ];
eta1Post = mPostV[ j + h * P ] / vPostV[ j + h * P ];
reps = nZerosAuxj[ j ] + (double) nOnesAuxj[ j ] *
nZerosSampling[ T - 1 ] / nOnesSampling[ T - 1 ];
eta2New = eta2Prior - 2 * reps * lambda(zeta) *
(mPostU[ i + h * T ] * mPostU[ i + h * T ] +
vPostU[ i + h * T ]);
eta1New = -(1 - 2 * e) * mPostU[ i + h * T ] *
reps / 2 + 2 * lambda(zeta) * reps *
mPostU[ i + h * T ] * (*mPred) + eta1Prior;
/* We update the posterior mean and variance */
eta2Post = (1 - rho) * eta2Post + rho * eta2New;
eta1Post = (1 - rho) * eta1Post + rho * eta1New;
vPostV[ j + h * P ] = 1 / eta2Post;
mPostV[ j + h * P ] = eta1Post / eta2Post;
/* We update the predictive mean and variance */
*mPred += mPostU[ i + h * T ] * mPostV[ j + h * P ];
*vPred += mPostU[ i + h * T ] * mPostU[ i + h * T ] *
vPostV[ j + h * P ] + vPostU[ i + h * T ] *
mPostV[ j + h * P ] * mPostV[ j + h * P ] +
vPostU[ i + h * T ] * vPostV[ j + h * P ];
/* We update the prior variance */
if (h != k - 1 && h != k - 2) {
vNew = ((mPostV[ j + h * P ] - mPriorV[ h ]) *
(mPostV[ j + h * P ] - mPriorV[ h ]) +
vPostV[ j + h * P ]) / vPriorVj[ j ];
vPriorVh[ h ] = (1 - rho) * vPriorVh[ h ] + rho * vNew;
}
}
}
Cell* eval(Cell* const c)
{
// when root cell is empty, throw an error.
judge_nil_cell(c,"begin");
// when root cell is a int or double cell, return a copy.
if (intp(c) || doublep(c) || procedurep(c)) {
return c -> deep_copy();
}
//if c is a symbol, there are several situations
//if c is in the local map, which means c is a procedurecell.
//if c is in the global map, which menas c is defined as some other value.
if (symbolp(c)) {
string var = get_symbol(c);
// first check if the symbol is defined at a local space
// if it is, then return a copy of it.
if (!my_stack.empty()) {
if (my_stack.top().count(var)) {
bstmap<string,Cell*> temp = my_stack.top();
if (nullp(temp[var])) return nil;
return temp[var] -> deep_copy();
}
}
// then check if the symbol is defined in the global map
if (symbol_table.count(var)) {
if (nullp(symbol_table[var])) return nil;
return symbol_table[var] -> deep_copy();
}
throw symbol_undefined_error("the variable " + var + " is not defined in the map");
}
// Then we know the 'c' cell must be a root of list
Cell* oper_cell = NULL;
string oper;
if (listp(car(c))) {
oper_cell = eval(car(c));
// oper cell should be the operator (primitive or procedure cell)
// it cannot be an empty operator
if (nullp(oper_cell)) {
throw invalid_operator_error("you cannot use an empty operator");
}
// if the first element of the expression is a list, the result evaluating it must be a procedure
if (!procedurep(oper_cell)) {
throw invalid_operator_error("cannot apply a value that is not a function");
}
// then return a copy of this procedure cell
if (procedurep(oper_cell)) {
Cell* argu_list = nullp(cdr(c)) ? nil : cdr(c) -> deep_copy();
return oper_cell -> apply(argu_list);
}
}
oper_cell = car(c) -> deep_copy();
// the operator cannot be a int or double
if (!symbolp(oper_cell)) {
throw invalid_operator_error("The input operator is not a procedure or primitive type");
}
oper = get_symbol(oper_cell);
/**
* the ceiling operator
* using oper_ceil(), which is a virtual function in Cell.
*/
if (oper == "ceiling") {
judge_num_argu(c,oper); // check validation
Cell* temp_cell = eval(car(cdr(c)));
judge_nil_cell(temp_cell,oper); // check validation
Cell* result = temp_cell -> oper_ceil();
delete oper_cell;
delete temp_cell;
return result;
}
/**
* the floor operator
* using oper_floor(), which is a virtual function in Cell.
*/
if (oper == "floor") {
judge_num_argu(c,oper); // check validation
Cell* temp_cell = eval(car(cdr(c)));
judge_nil_cell(temp_cell,oper); //check validation
Cell* result = temp_cell -> oper_floor();
delete oper_cell;
delete temp_cell;
return result;
}
/**
* the add operator
* using oper_add(), which is a virtual function in Cell.
*/
if (oper == "+") {
// by "+" convention, when evaluate a single plus operator, return 0.
if (nullp(cdr(c))) {
delete oper_cell;
return make_int(0);
}
int size = cdr(c) -> cons_size();
// use cell:result as a initial cell
Cell* result = make_int(0);
Cell* next_elem = cdr(c);
for (int i=1; i<=size; i++) {
Cell* next_temp = eval(car(next_elem));
judge_nil_cell(next_temp,oper);
result = result -> oper_add(next_temp);
delete next_temp; //delete temporary cell in every step.
next_elem = cdr(next_elem);
}
delete oper_cell;
return result;
}
/**
* the minus operator
* using oper_minus(), which is a virtual function in Cell.
*/
if (oper == "-") {
judge_num_argu(c,oper);
int size = cdr(c) -> cons_size();
Cell* result = eval(car(cdr(c)));
judge_nil_cell(result,oper);
// by "-" convention, when only one operand, return its inverse.
if (size == 1) {
result = result -> oper_minus(nil);
delete oper_cell;
return result;
}
Cell* next_elem = cdr(cdr(c));
for (int i=1; i<size; i++) {
Cell* next_temp = eval(car(next_elem));
judge_nil_cell(next_temp,oper);
result = result -> oper_minus(next_temp);
delete next_temp; //delete temporary cell in every step.
next_elem = cdr(next_elem);
}
delete oper_cell;
return result;
}
/**
* the multiply operator
* using oper_multiply(), which is a virtual function in Cell.
*/
if (oper == "*") {
if (nullp(cdr(c))) {
delete oper_cell;
return make_int(1);
}
int size = cdr(c) -> cons_size();
Cell* result = make_int(1);
Cell* next_elem = cdr(c);
for (int i=1; i<=size; i++) {
Cell* next_temp = eval(car(next_elem));
judge_nil_cell(next_temp,oper);
result = result -> oper_multiply(next_temp);
delete next_temp; //delete temporary cell in every step.
next_elem = cdr(next_elem);
}
delete oper_cell;
return result;
}
/**
* the division operator
* using oper_divide(), which is a virtual function in Cell.
*/
if (oper == "/") {
judge_num_argu(c,oper);
int size = cdr(c) -> cons_size();
Cell* result = eval(car(cdr(c)));
judge_nil_cell(result,oper);
// by "/" convention, when only one operand, return its inverse.
if (size == 1) {
result = result -> oper_divide(nil);
delete oper_cell;
return result;
}
Cell* next_elem = cdr(cdr(c));
for (int i=1; i<size; i++) {
Cell* next_temp = eval(car(next_elem));
judge_nil_cell(next_temp,oper);
result = result -> oper_divide(next_temp);
delete next_temp; //delete temporary cell on every step.
next_elem = cdr(next_elem);
}
delete oper_cell;
return result;
}
/**
* the if operator
* using oper_if(), which is a virtual function in Cell.
*/
if (oper == "if") {
judge_num_argu(c,oper); // check validation
int size = cdr(c) -> cons_size();
Cell* judge_cell = cdr(c); // the first operand after if
Cell* judge_cell_temp = eval(car(judge_cell));
Cell* true_cell = cdr(judge_cell); // the second operand after if
// if judge cell is a empty list, return true_cell
if (nullp(judge_cell_temp)) {
delete judge_cell_temp;
delete oper_cell;
return eval(car(true_cell));
}
Cell* condition = judge_cell_temp -> oper_if();
// condition must be an IntCell from the implementation of Cell.cpp
if (get_int(condition)) {
Cell* true_cell_temp = eval(car(true_cell));
delete judge_cell_temp;
delete oper_cell;
return true_cell_temp;
} else {
Cell* false_cell = nil;
// if there are only two operand, which is an undefined behavior
// we will let it return the second operand
if (size == 2) {
false_cell = eval(car(true_cell));
} else {
false_cell = eval(car(cdr(true_cell)));
}
delete judge_cell_temp;
delete oper_cell;
return false_cell;
}
}
/**
* the quote operator
*/
if (oper == "quote") {
judge_num_argu(c,oper); // check validation
delete oper_cell;
if (nullp(car(cdr(c)))) return nil;
return car(cdr(c)) -> deep_copy();
}
/**
* the cons operator
* using cons() which is a function in cons.hpp.
*/
if (oper == "cons") {
judge_num_argu(c,oper); // check validation
Cell* car_new = eval(car(cdr(c)));
Cell* cdr_new = eval(car(cdr(cdr(c))));
if (!listp(cdr_new)) {
throw invalid_operator_error("cdr must either be nil or a conspair");
}
Cell* result = cons(car_new, cdr_new);
delete oper_cell;
return result;
}
/**
* the car operator
* using car() which is a function in cons.hpp.
*/
if (oper == "car") {
judge_num_argu(c,oper);
Cell* temp = eval(car(cdr(c)));
Cell* result = nullp(car(temp)) ? nil : car(temp) -> deep_copy();
delete temp;
delete oper_cell;
return result;
}
/**
* the cdr operator
* using cdr() which is a function in cons.hpp.
*/
if (oper == "cdr") {
judge_num_argu(c,oper);
Cell* temp = eval(car(cdr(c)));
Cell* result = nullp(cdr(temp)) ? nil : cdr(temp) -> deep_copy();
delete temp;
delete oper_cell;
return result;
}
/**
* the nullp operator
* using nullp() which is a function in cons.hpp.
*/
if (oper == "nullp") {
judge_num_argu(c,oper);
Cell* temp_cell = eval(car(cdr(c)));
if(nullp(temp_cell)) {
delete temp_cell;
delete oper_cell;
return make_int(1);
} else {
delete temp_cell;
delete oper_cell;
return make_int(0);
}
}
/**
* the define operator
* using map to record relation between string and cell
*/
if (oper == "define") {
judge_num_argu(c,oper);
Cell* key_cell = car(cdr(c));
Cell* next_cell = car(cdr(cdr(c)));
if (!symbolp(key_cell)) {
throw operate_on_nil_error("define operand must be a symbol");
}
Cell* mapped_cell = eval(next_cell);
string key = get_symbol(key_cell);
if (symbol_table.count(key)) {
throw invalid_operand_error("Cannot redefine a variable");
}
symbol_table.insert(make_pair(key,mapped_cell));
delete oper_cell;
return nil;
}
/**
* the less operator
* using oper_less(), which is a virtual function in Cell.
*/
if (oper == "<") {
//by convention, it will return 1 when zero argument.
if (nullp(cdr(c))) {
return make_int(1);
}
int size = cdr(c) -> cons_size();
//when one argument, return itself.
if (size == 1) {
Cell* result = eval(car(cdr(c)));
judge_nil_cell(result,oper);
result = result -> oper_less(nil);
delete oper_cell;
return result;
}
Cell* this_elem = cdr(c);
Cell* next_elem = cdr(cdr(c));
int condition = 1;
for (int i=1; i<size; i++) {
Cell* next_temp = eval(car(next_elem));
judge_nil_cell(next_temp,oper);
Cell* this_temp = eval(car(this_elem));
judge_nil_cell(this_temp,oper);
Cell* judge_cell = this_temp -> oper_less(next_temp);
// if there is one pair such that 'this' bigger than 'next', condition will be zero
condition *= get_int(judge_cell);
// delete temporary cell
delete judge_cell;
delete this_temp;
delete next_temp;
this_elem = cdr(this_elem);
next_elem = cdr(next_elem);
}
delete oper_cell;
return make_int(condition);
}
/**
* the not operator
* using oper_not(), which is a virtual function in Cell.
*/
if (oper == "not") {
judge_num_argu(c,oper);
Cell* operand = eval(car(cdr(c)));
if (nullp(operand)) {
delete oper_cell;
return make_int(0);
}
Cell* result = operand -> oper_not();
delete operand;
delete oper_cell;
return result;
}
/**
* the print operator
* print the result and return nil cell
*/
if (oper == "print") {
judge_num_argu(c,oper);
Cell* result = eval(car(cdr(c)));
if (nullp(result)) {
cout << "()" << endl;
return nil;
}
cout << *result << endl;
delete oper_cell;
delete result;
return nil;
}
/**
* the eval operator
* evaluate the result and return nil
*/
if (oper == "eval") {
judge_num_argu(c,oper);
Cell* expr = eval(car(cdr(c)));
Cell* result = eval(expr);
delete oper_cell;
return result;
}
/**
* the lambda operator
* a new function
*/
if (oper == "lambda") {
judge_num_argu(c,oper);
Cell* formal_m = nullp(car(cdr(c))) ? nil : car(cdr(c)) -> deep_copy();
// since the formal part must be a symbol or list(can be empty)
if (!listp(formal_m) && !symbolp(formal_m) && !nullp(formal_m)) {
throw invalid_operand_error("the type of formal part should be list or symbol");
}
Cell* body_m = nullp(cdr(cdr(c))) ? nil : cdr(cdr(c)) -> deep_copy();
delete oper_cell;
return lambda(formal_m,body_m);
}
/**
* the apply operator
* followed by a function and a list of arguments
*/
if (oper == "apply") {
judge_num_argu(c,oper);
Cell* procedure = eval(car(cdr(c)));
if (!procedurep(procedure)) {
throw invalid_operator_error("cannot apply a value that is not a function");
}
Cell* argu_list = eval(car(cdr(cdr(c))));
if (!listp(argu_list)) {
throw invalid_operand_error("the second operand after apply function must be a list");
}
Cell* result = procedure -> apply(argu_list);
delete procedure;
delete argu_list;
delete oper_cell;
return result;
}
/*
* the let operator
* which allow define local variable before function definition
*/
if (oper == "let") {
judge_num_argu(c,oper);
Cell* var_definition = car(cdr(c));
Cell* func_body = car(cdr(cdr(c)));
bstmap<string,Cell*> local_map;
// According to the specification, the variable definition part must be list
if (!listp(var_definition)) {
throw invalid_operand_error("In let function, the varible definition must be a list");
}
int size = car(var_definition) -> cons_size();
for (int i=0; i<size; i++) {
Cell* begin_cell = car(var_definition);
if (!listp(begin_cell)) {
throw invalid_operand_error("In let function, the varible definition must be a list");
}
int sub_size = begin_cell -> cons_size();
// When you want to define a variable, you can only give a symbol and a value, so the size should be 2
if (sub_size != 2) {
throw wrong_num_argu_error("When define local variable, the size of list must be 2");
}
string key = get_symbol(car(begin_cell));
Cell* argu = car(cdr(begin_cell));
local_map.insert(make_pair(key,argu));
var_definition = cdr(var_definition);
}
my_stack.push(local_map);
return eval(func_body);
}
// If the oper is not the above primitive operator, then check whether they are in the global map
// If it is, follow the similar step declared above
if (symbol_table.count(oper)) {
if (procedurep(symbol_table[oper])) {
Cell* procedure = symbol_table[oper] -> deep_copy();
Cell* argu_list = nullp(cdr(c)) ? nil : cdr(c) -> deep_copy();
Cell* result = procedure -> apply(argu_list);
delete oper_cell;
delete procedure;
delete argu_list;
return result;
}
}
throw invalid_operator_error("this operator is invalid");
}
/* fix narrow-lane ambiguity by ILS ------------------------------------------*/
static int fix_amb_ILS(rtk_t *rtk, int *sat1, int *sat2, int *NW, int n)
{
double C1,C2,*B1,*N1,*NC,*D,*E,*Q,s[2],lam_NL=lam_LC(1,1,0),lam1,lam2;
int i,j,k,m=0,info,stat,flgs[MAXSAT]={0},max_flg=0;
lam1=lam_carr[0]; lam2=lam_carr[1];
C1= SQR(lam2)/(SQR(lam2)-SQR(lam1));
C2=-SQR(lam1)/(SQR(lam2)-SQR(lam1));
B1=zeros(n,1); N1=zeros(n,2); D=zeros(rtk->nx,n); E=mat(n,rtk->nx);
Q=mat(n,n); NC=mat(n,1);
for (i=0;i<n;i++) {
/* check linear independency */
if (!is_depend(sat1[i],sat2[i],flgs,&max_flg)) continue;
j=IB(sat1[i],&rtk->opt);
k=IB(sat2[i],&rtk->opt);
/* float narrow-lane ambiguity (cycle) */
B1[m]=(rtk->x[j]-rtk->x[k]+C2*lam2*NW[i])/lam_NL;
N1[m]=ROUND(B1[m]);
/* validation of narrow-lane ambiguity */
if (fabs(N1[m]-B1[m])>rtk->opt.thresar[2]) continue;
/* narrow-lane ambiguity transformation matrix */
D[j+m*rtk->nx]= 1.0/lam_NL;
D[k+m*rtk->nx]=-1.0/lam_NL;
sat1[m]=sat1[i];
sat2[m]=sat2[i];
NW[m++]=NW[i];
}
if (m<3) return 0;
/* covariance of narrow-lane ambiguities */
matmul("TN",m,rtk->nx,rtk->nx,1.0,D,rtk->P,0.0,E);
matmul("NN",m,m,rtk->nx,1.0,E,D,0.0,Q);
/* integer least square */
if ((info=lambda(m,2,B1,Q,N1,s))) {
trace(2,"lambda error: info=%d\n",info);
return 0;
}
if (s[0]<=0.0) return 0;
rtk->sol.ratio=(float)(MIN(s[1]/s[0],999.9));
/* varidation by ratio-test */
if (rtk->opt.thresar[0]>0.0&&rtk->sol.ratio<rtk->opt.thresar[0]) {
trace(2,"varidation error: n=%2d ratio=%8.3f\n",m,rtk->sol.ratio);
return 0;
}
trace(2,"varidation ok: %s n=%2d ratio=%8.3f\n",time_str(rtk->sol.time,0),m,
rtk->sol.ratio);
/* narrow-lane to iono-free ambiguity */
for (i=0;i<m;i++) {
NC[i]=C1*lam1*N1[i]+C2*lam2*(N1[i]-NW[i]);
}
/* fixed solution */
stat=fix_sol(rtk,sat1,sat2,NC,m);
free(B1); free(N1); free(D); free(E); free(Q); free(NC);
return stat;
}
