void mrpt::vision::pnp::posit::POS()
{
Eigen::Vector3d I0, J0 , r1, r2, r3;
double I0_norm, J0_norm;
int i;
double scale;
for(i=0;i<3;i++)
{
I0(i)=obj_matrix.row(i).dot(img_vecs.col(0));
J0(i)=obj_matrix.row(i).dot(img_vecs.col(1));
}
I0_norm=I0.norm();
J0_norm=J0.norm();
scale=(I0_norm + J0_norm)/2;
/*Computing TRANSLATION */
t(0)=img_pts(0,0)/scale;
t(1)=img_pts(0,1)/scale;
t(2)=f/scale;
/* Computing ROTATION */
r1=I0/I0_norm;
r2=J0/J0_norm;
r3=r1.cross(r2);
R.row(0)=r1;
R.row(1)=r2;
R.row(2)=r3;
}
void bessel_i_scaled(_FLOAT_ x, _FLOAT_ i[], int n, int n1)
/* computes i_n(x)/x^(n+1) */
{
int l;
_FLOAT_ s, x2;
x2 = x*x;
i[n1-1] = 0.0;
i[n1-2] = 1.0;
for (l=n1-3; l>=0; l--) {
#if (FMM_PRECISION==0)
if (l==n-3) {
i[l+2]/=i[l+1];
i[l+1]=1.0;
}
#endif
i[l] = x2*i[l+2] + (2*l+2)*i[l+1];
}
s = I0(x)/(x*i[0]);
for (l=0;l<n; l++) {
i[l] *= s;
}
}
float HairBcsdf::logI0(float x)
{
if (x > 12.0f)
// More stable evaluation of log(I0(x))
// See also https://publons.com/discussion/12/
return x + 0.5f*(std::log(1.0f/(TWO_PI*x)) + 1.0f/(8.0f*x));
else
return std::log(I0(x));
}
double K0(double z){//0th Modified Bessel Function of the second kind
z=fabs(z/2);
if(z<=1)
return -log(z)*I0(2*z)-0.57721566+0.42278420*z*z+0.23069756*pow(z,4)+
0.03488590*pow(z,6)+0.00262698*pow(z,8)+0.00010750*pow(z,10)+
0.00000740*pow(z,12);
else
return exp(-2*z)/sqrt(2*z)*(1.25331414-0.07832358/z+0.02189568/z/z-
0.01062446/z/z/z+0.00587872/z/z/z/z-
0.00251540/pow(z,5)+0.00053208/pow(z,6));
}
void dist_chamfer_cabl(Im2D<U_INT1,INT> I,INT v_max)
{
Im2D<U_INT1,INT> I0(I.tx(),I.ty(),0);
ELISE_COPY(I0.all_pts(),I.in(),I0.out());
Chamfer::d32.im_dist(I);
INT nb_dif;
ELISE_COPY
(
I.all_pts(),
I0.in()!=(I.in()!=0),
sigma(nb_dif)
);
BENCH_ASSERT(nb_dif == 0);
INT tx = I.tx();
INT ty = I.ty();
U_INT1 ** d = I.data();
INT vmax = I.vmax()-1;
for (int x=1; x<tx-1 ; x++)
for (int y=1; y<ty-1 ; y++)
{
INT v;
if (d[y][x])
v = std::min3
(
std::min3(d[y+1][x-1]+3,d[y+1][x]+2,d[y+1][x+1]+3),
std::min3(d[y][x-1]+2,vmax,d[y][x+1]+2),
std::min3(d[y-1][x-1]+3,d[y-1][x]+2,d[y-1][x+1]+3)
);
else
v = 0;
BENCH_ASSERT(v == d[y][x]);
}
INT dif;
ELISE_COPY
(
I.all_pts(),
Abs
(
Min(I.in(),v_max)
- extinc_32(I0.in(0),v_max)
),
VMax(dif)
);
BENCH_ASSERT(dif == 0);
}
// Rough longitudinal scattering function with variance v = beta^2
float HairBcsdf::M(float v, float sinThetaI, float sinThetaO, float cosThetaI, float cosThetaO) const
{
float a = cosThetaI*cosThetaO/v;
float b = sinThetaI*sinThetaO/v;
if (v < 0.1f)
// More numerically stable evaluation for small roughnesses
// See https://publons.com/discussion/12/
return std::exp(-b + logI0(a) - 1.0f/v + 0.6931f + std::log(1.0f/(2.0f*v)));
else
return std::exp(-b)*I0(a)/(2.0f*v*std::sinh(1.0f/v));
}
double IN(int n,double z){
double i0=I0(z),i1=I1(z),in=0;
int i;
if(n==0)return i0;
if(n==1)return i1;
for(i=1;i<n;i++){
in=i0-(2./z)*i*i1;
i0=i1;
i1=in;
}
return in;
}
void bessel_i(_FLOAT_ x, _FLOAT_ i[], int n, int n1)
{
int l;
_FLOAT_ one_over_x, s;
one_over_x = 1.0/x;
i[n1-1] = 0.0;
i[n1-2] = 1.0;
for (l=n1-3; l>=0; l--) {
i[l] = i[l+2] + (2*l+2)*i[l+1]*one_over_x;
}
s = I0(x)/i[0];
for (l=0;l<n; l++) {
i[l] *= s;
}
}
bool FragmentList_checkEntryExists(FragmentNode *fragmentNode, uint64 offset, uint64 length)
{
bool existsFlag;
uint64 i0,i1;
FragmentEntryNode *fragmentEntryNode;
assert(fragmentNode != NULL);
i0 = I0(offset,length);
i1 = I1(offset,length);
existsFlag = FALSE;
for (fragmentEntryNode = fragmentNode->fragmentEntryList.head; (fragmentEntryNode != NULL) && !existsFlag; fragmentEntryNode = fragmentEntryNode->next)
{
if ( ((F0(fragmentEntryNode) <= i0) && (i0 <= F1(fragmentEntryNode)) )
|| ((F0(fragmentEntryNode) <= i1) && (i1 <= F1(fragmentEntryNode)))
)
{
existsFlag = TRUE;
}
}
return existsFlag;
}
void FragmentList_addEntry(FragmentNode *fragmentNode, uint64 offset, uint64 length)
{
FragmentEntryNode *fragmentEntryNode,*deleteFragmentEntryNode;
FragmentEntryNode *prevFragmentEntryNode,*nextFragmentEntryNode;
assert(fragmentNode != NULL);
/* remove all fragments which are completely covered by new fragment */
fragmentEntryNode = fragmentNode->fragmentEntryList.head;
while (fragmentEntryNode != NULL)
{
if ((F0(fragmentEntryNode) >= I0(offset,length)) && (F1(fragmentEntryNode) <= I1(offset,length)))
{
deleteFragmentEntryNode = fragmentEntryNode;
fragmentEntryNode = fragmentEntryNode->next;
List_remove(&fragmentNode->fragmentEntryList,deleteFragmentEntryNode);
LIST_DELETE_NODE(deleteFragmentEntryNode);
}
else
{
fragmentEntryNode = fragmentEntryNode->next;
}
}
/* find prev/next fragment */
prevFragmentEntryNode = NULL;
fragmentEntryNode = fragmentNode->fragmentEntryList.head;
while ((fragmentEntryNode != NULL) && (F1(fragmentEntryNode) < I1(offset,length)))
{
prevFragmentEntryNode = fragmentEntryNode;
fragmentEntryNode = fragmentEntryNode->next;
}
nextFragmentEntryNode = NULL;
fragmentEntryNode = fragmentNode->fragmentEntryList.tail;
while ((fragmentEntryNode != NULL) && (F0(fragmentEntryNode) > I0(offset,length)))
{
nextFragmentEntryNode = fragmentEntryNode;
fragmentEntryNode = fragmentEntryNode->prev;
}
/* check if existing Fragment can be extended or new Fragment have to be inserted */
if ((prevFragmentEntryNode != NULL) && (F1(prevFragmentEntryNode)+1 >= I0(offset,length)))
{
/* combine with previous existing fragment */
prevFragmentEntryNode->length = (offset+length)-prevFragmentEntryNode->offset;
prevFragmentEntryNode->offset = prevFragmentEntryNode->offset;
}
else if ((nextFragmentEntryNode != NULL) && (I1(offset,length)+1 >= F0(nextFragmentEntryNode)))
{
/* combine with next existing fragment */
nextFragmentEntryNode->length = (nextFragmentEntryNode->offset+nextFragmentEntryNode->length)-offset;
nextFragmentEntryNode->offset = offset;
}
else
{
/* insert new Fragment */
fragmentEntryNode = LIST_NEW_NODE(FragmentEntryNode);
if (fragmentEntryNode == NULL)
{
HALT_INSUFFICIENT_MEMORY();
}
fragmentEntryNode->offset = offset;
fragmentEntryNode->length = length;
List_insert(&fragmentNode->fragmentEntryList,fragmentEntryNode,nextFragmentEntryNode);
}
}
int
vpPose::calculArbreDementhon(vpMatrix &b, vpColVector &U,
vpHomogeneousMatrix &cMo)
{
#if (DEBUG_LEVEL1)
std::cout << "begin vpPose::CalculArbreDementhon() " << std::endl;
#endif
unsigned int i, k;
int erreur = 0;
unsigned int cpt;
double s,c,si,co;
double smin,smin_old, s1,s2;
double r, theta;
vpHomogeneousMatrix cMo1,cMo2,cMo_old;
unsigned int iter_max = 20;
vpMatrix eps(iter_max+1,npt) ;
// on test si tous les points sont devant la camera
for(i = 0; i < npt; i++)
{
double z ;
z = cMo[2][0]*c3d[i].get_oX()+cMo[2][1]*c3d[i].get_oY()+cMo[2][2]*c3d[i].get_oZ() + cMo[2][3];
if (z <= 0.0) erreur = -1;
}
smin = sqrt(computeResidualDementhon(cMo)/npt) ;
vpColVector xi(npt) ;
vpColVector yi(npt) ;
if (erreur==0)
{
k=0;
for(i = 0; i < npt; i++)
{
xi[k] = c3d[i].get_x();
yi[k] = c3d[i].get_y();
if (k != 0)
{ // On ne prend pas le 1er point
eps[0][k] = (cMo[2][0]*c3d[i].get_oX() +
cMo[2][1]*c3d[i].get_oY() +
cMo[2][2]*c3d[i].get_oZ())/cMo[2][3];
}
k++;
}
vpColVector I0(3) ;
vpColVector J0(3) ;
vpColVector I(3) ;
vpColVector J(3) ;
vpHomogeneousMatrix cMo_old ;
smin_old = 2*smin ;
cpt = 0;
while ((cpt<20) && (smin_old > 0.01) && (smin <= smin_old))
{
#if (DEBUG_LEVEL2)
{
std::cout << "cpt " << cpt << std::endl ;
std::cout << "smin_old " << smin_old << std::endl ;
std::cout << "smin " << smin << std::endl ;
}
#endif
smin_old = smin;
cMo_old = cMo;
I0 = 0 ;
J0 = 0 ;
for (i=1;i<npt;i++)
{
s = (1.0+eps[cpt][i])*xi[i] - xi[0];
I0[0] += b[0][i-1] * s;
I0[1] += b[1][i-1] * s;
I0[2] += b[2][i-1] * s;
s = (1.0+eps[cpt][i])*yi[i] - yi[0];
J0[0] += b[0][i-1] * s;
J0[1] += b[1][i-1] * s;
J0[2] += b[2][i-1] * s;
}
s = -2.0*(vpColVector::dotProd(I0,J0));
c = J0.sumSquare() - I0.sumSquare() ;
calculRTheta(s,c,r,theta);
co = cos(theta);
si = sin(theta);
/* 1ere branche */
I = I0 + U*r*co ;
J = J0 + U*r*si ;
#if (DEBUG_LEVEL3)
{
std::cout << "I " << I.t() ;
std::cout << "J " << J.t() ;
}
#endif
calculSolutionDementhon(xi[0],yi[0],I,J,cMo1);
s1 = sqrt(computeResidualDementhon(cMo1)/npt) ;
#if (DEBUG_LEVEL3)
std::cout << "cMo1 "<< std::endl << cMo1 << std::endl ;
#endif
/* 2eme branche */
I = I0 - U*r*co ;
J = J0 - U*r*si ;
#if (DEBUG_LEVEL3)
{
std::cout << "I " << I.t() ;
std::cout << "J " << J.t() ;
}
#endif
calculSolutionDementhon(xi[0],yi[0],I,J,cMo2);
s2 = sqrt(computeResidualDementhon(cMo2)/npt) ;
#if (DEBUG_LEVEL3)
std::cout << "cMo2 "<< std::endl << cMo2 << std::endl ;
#endif
cpt ++;
if (s1 <= s2)
{
smin = s1;
k = 0;
for(i = 0; i < npt; i++)
{
if (k != 0) { // On ne prend pas le 1er point
eps[cpt][k] = (cMo1[2][0]*c3d[i].get_oX() + cMo1[2][1]*c3d[i].get_oY()
+ cMo1[2][2]*c3d[i].get_oZ())/cMo1[2][3];
}
k++;
}
cMo = cMo1 ;
}
else
{
smin = s2;
k = 0;
for(i = 0; i < npt; i++)
{
if (k != 0) { // On ne prend pas le 1er point
eps[cpt][k] = (cMo2[2][0]*c3d[i].get_oX() + cMo2[2][1]*c3d[i].get_oY()
+ cMo2[2][2]*c3d[i].get_oZ())/cMo2[2][3];
}
k++;
}
cMo = cMo2 ;
}
if (smin > smin_old)
{
#if (DEBUG_LEVEL2)
std::cout << "Divergence " << std::endl ;
#endif
cMo = cMo_old ;
}
#if (DEBUG_LEVEL2)
{
std::cout << "s1 = " << s1 << std::endl ;
std::cout << "s2 = " << s2 << std::endl ;
std::cout << "smin = " << smin << std::endl ;
std::cout << "smin_old = " << smin_old << std::endl ;
}
#endif
}
}
#if (DEBUG_LEVEL1)
std::cout << "end vpPose::CalculArbreDementhon() return "<< erreur << std::endl;
#endif
return erreur ;
}
void bench_algo_dist_32(Pt2di sz)
{
Im2D_U_INT1 I0(sz.x,sz.y);
Im2D_U_INT1 Id_plus(sz.x,sz.y);
Im2D_U_INT1 Id_moins(sz.x,sz.y);
Im2D_INT1 Idneg1(sz.x,sz.y);
Im2D_INT1 Idneg2(sz.x,sz.y);
ELISE_COPY
(
I0.all_pts(),
gauss_noise_1(10)>0.5,
I0.out()
);
ELISE_COPY(I0.border(2),0,I0.out());
ELISE_COPY
(
I0.all_pts(),
extinc_32(I0.in(0),255),
Id_plus.out()
);
ELISE_COPY
(
I0.all_pts(),
extinc_32(! I0.in(0),255),
Id_moins.out()
);
ELISE_COPY
(
select(I0.all_pts(),I0.in()),
Min(Id_plus.in()-1,127),
Idneg1.out()
);
ELISE_COPY
(
select(I0.all_pts(),! I0.in()),
Max(1-Id_moins.in(),-128),
Idneg1.out()
);
ELISE_COPY(I0.all_pts(),I0.in(),Idneg2.out());
TIm2D<INT1,INT> TIdneg2(Idneg2);
TIdneg2.algo_dist_32_neg();
INT dif;
ELISE_COPY
(
I0.all_pts(),
Abs(Idneg2.in()-Idneg1.in()),
VMax(dif)
);
BENCH_ASSERT(dif==0);
ELISE_COPY
(
Idneg1.all_pts(),
Iconv((gauss_noise_1(10)-0.5)* 20),
Idneg1.out() | Idneg2.out()
);
ELISE_COPY(Idneg1.border(2),-100, Idneg1.out() | Idneg2.out());
ELISE_COPY
(
Idneg1.all_pts(),
128-EnvKLipshcitz_32(128-Idneg1.in(-100),100),
Idneg1.out()
);
TIdneg2.algo_dist_env_Klisp32_Sup();
INT Max2 [2];
INT Min2 [2];
ELISE_COPY
(
I0.interior(1),
Virgule(Idneg1.in(),Idneg2.in()),
VMax(Max2,2) | VMin(Min2,2)
);
ELISE_COPY
(
I0.interior(1),
Abs(Idneg2.in()-Idneg1.in()),
VMax(dif)
);
BENCH_ASSERT(dif==0);
}
static double kaiser(int k, int N, double beta) {
if (k < 0 || k > N)
return 0;
return I0(beta * sqrt(1.0 - sqr((2.0*k)/N - 1.0))) / I0(beta);
}
void
vpPose::poseDementhonPlan(vpHomogeneousMatrix &cMo)
{
#if (DEBUG_LEVEL1)
std::cout << "begin CCalculPose::PoseDementhonPlan()" << std::endl ;
#endif
unsigned int i,j,k ;
if (c3d !=NULL) delete []c3d ;
c3d = new vpPoint[npt] ;
vpPoint p0 = listP.front() ;
vpPoint P ;
i=0 ;
for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
{
P = *it;
c3d[i] = P ;
c3d[i].set_oX(P.get_oX()-p0.get_oX()) ;
c3d[i].set_oY(P.get_oY()-p0.get_oY()) ;
c3d[i].set_oZ(P.get_oZ()-p0.get_oZ()) ;
i++ ;
}
vpMatrix a ;
try
{
a.resize(npt-1,3) ;
}
catch(...)
{
vpERROR_TRACE(" ") ;
throw ;
}
for (i=1 ; i < npt ; i++)
{
a[i-1][0]=c3d[i].get_oX();
a[i-1][1]=c3d[i].get_oY();
a[i-1][2]=c3d[i].get_oZ();
}
// calcul a^T a
vpMatrix ata ;
ata = a.t()*a ;
/* essai FC pour debug SVD */
/*
vpMatrix ata_old ;
ata_old = a.t()*a ;
vpMatrix ata((ata_old.getRows()-1),(ata_old.getCols()-1)) ;
for (i=0;i<ata.getRows();i++)
for (j=0;j<ata.getCols();j++) ata[i][j] = ata_old[i][j];
*/
vpMatrix ata_sav;
ata_sav = ata;
#if (DEBUG_LEVEL2)
{
std::cout << "a" << std::endl <<a<<std::endl ;
std::cout << "ata" << std::endl <<ata<<std::endl ;
}
#endif
// calcul (a^T a)^-1
vpMatrix ata1(ata.getRows(),ata.getCols()) ;
vpMatrix v(ata.getRows(),ata.getCols());
vpColVector sv(ata.getRows());
// ata1 = ata.i() ;
unsigned int imin = 0;
double s = 0.0;
//calcul de ata^-1
ata.svd(sv,v) ;
unsigned int nc = sv.getRows() ;
for (i=0; i < nc ; i++)
if (sv[i] > s) s = sv[i];
s *= 0.0002;
int irank = 0;
for (i=0;i<nc;i++)
if (sv[i] > s ) irank++;
double svm = 100.0;
for (i = 0; i < nc; i++)
if (sv[i] < svm) { imin = i; svm = sv[i]; }
#if (DEBUG_LEVEL2)
{
std::cout << "rang: " << irank << std::endl ;;
std::cout <<"imin = " << imin << std::endl ;
std::cout << "sv " << sv.t() << std::endl ;
}
#endif
for (i=0 ; i < ata.getRows() ; i++)
for (j=0 ; j < ata.getCols() ; j++)
{
ata1[i][j] = 0.0;
for (k=0 ; k < nc ; k++)
if (sv[k] > s)
ata1[i][j] += ((v[i][k]*ata[j][k])/sv[k]);
}
vpMatrix b ; // b=(at a)^-1*at
b = ata1*a.t() ;
//calcul de U
vpColVector U(3) ;
U = ata.column(imin+1) ;
#if (DEBUG_LEVEL2)
{
std::cout << "a" << std::endl <<a<<std::endl ;
std::cout << "ata" << std::endl <<ata_sav<<std::endl ;
std::cout << "ata1" << std::endl <<ata1<<std::endl ;
std::cout << "ata1*ata" << std::endl << ata1*ata_sav ;
std::cout << "b" << std::endl << b ;
std::cout << "U " << U.t() << std::endl ;
}
#endif
vpColVector xi(npt) ;
vpColVector yi(npt) ;
//calcul de la premiere solution
for (i = 0; i < npt; i++)
{
xi[i] = c3d[i].get_x() ;
yi[i] = c3d[i].get_y() ;
}
vpColVector I0(3) ; I0 = 0 ;
vpColVector J0(3) ; J0 = 0 ;
vpColVector I(3) ;
vpColVector J(3) ;
for (i=1;i<npt;i++)
{
I0[0] += b[0][i-1] * (xi[i]-xi[0]);
I0[1] += b[1][i-1] * (xi[i]-xi[0]);
I0[2] += b[2][i-1] * (xi[i]-xi[0]);
J0[0] += b[0][i-1] * (yi[i]-yi[0]);
J0[1] += b[1][i-1] * (yi[i]-yi[0]);
J0[2] += b[2][i-1] * (yi[i]-yi[0]);
}
#if (DEBUG_LEVEL2)
{
std::cout << "I0 "<<I0.t() ;
std::cout << "J0 "<<J0.t() ;
}
#endif
s = -2.0*vpColVector::dotProd(I0,J0);
double c = J0.sumSquare() - I0.sumSquare() ;
double r,theta,si,co ;
calculRTheta(s, c, r, theta);
co = cos(theta);
si = sin(theta);
// calcul de la premiere solution
I = I0 + U*r*co ;
J = J0 + U*r*si ;
vpHomogeneousMatrix cMo1f ;
calculSolutionDementhon(xi[0], yi[0], I, J, cMo1f);
int erreur1 = calculArbreDementhon(b, U, cMo1f);
// calcul de la deuxieme solution
I = I0 - U*r*co ;
J = J0 - U*r*si ;
vpHomogeneousMatrix cMo2f;
calculSolutionDementhon(xi[0], yi[0], I, J, cMo2f);
int erreur2 = calculArbreDementhon(b, U, cMo2f);
if ((erreur1 == 0) && (erreur2 == -1)) cMo = cMo1f ;
if ((erreur1 == -1) && (erreur2 == 0)) cMo = cMo2f ;
if ((erreur1 == 0) && (erreur2 == 0))
{
double s1 = sqrt(computeResidualDementhon(cMo1f)/npt) ;
double s2 = sqrt(computeResidualDementhon(cMo2f)/npt) ;
if (s1<=s2) cMo = cMo1f ; else cMo = cMo2f ;
}
cMo[0][3] -= p0.get_oX()*cMo[0][0]+p0.get_oY()*cMo[0][1]+p0.get_oZ()*cMo[0][2];
cMo[1][3] -= p0.get_oX()*cMo[1][0]+p0.get_oY()*cMo[1][1]+p0.get_oZ()*cMo[1][2];
cMo[2][3] -= p0.get_oX()*cMo[2][0]+p0.get_oY()*cMo[2][1]+p0.get_oZ()*cMo[2][2];
delete [] c3d ; c3d = NULL ;
#if (DEBUG_LEVEL1)
std::cout << "end CCalculPose::PoseDementhonPlan()" << std::endl ;
#endif
}
std::vector<element_type const *> search(element_type const & elem) const
{
std::vector<element_type const *> R;
typedef std::pair<uint64_t,uint64_t> upair;
typedef std::pair<int,upair> q_element;
std::deque< q_element > todo;
if ( elem.getTo() > elem.getFrom() )
todo.push_back(q_element(k,upair(elem.getFrom(),elem.getTo())));
while ( todo.size() )
{
q_element q = todo.front();
todo.pop_front();
int const level = k-q.first;
uint64_t const basebin = (1ull << level)-1;
uint64_t const div = (1ull<<(q.first+1));
uint64_t const mask = (q.first >= 0) ? (1<<q.first) : 0;
uint64_t const low = q.second.first;
uint64_t const high = q.second.second;
uint64_t const bin = basebin + low/div;
if ( bins.find(bin) != bins.end() )
{
std::vector<element_type> const & V = bins.find(bin)->second;
for ( uint64_t i = 0; i < V.size(); ++i )
{
libmaus2::math::IntegerInterval<uint64_t> I0(elem.getFrom(),elem.getTo()-1);
libmaus2::math::IntegerInterval<uint64_t> I1(V[i].getFrom(),V[i].getTo()-1);
if ( !I0.intersection(I1).isEmpty() )
R.push_back(&V[i]);
}
}
// std::cerr << level << "," << q.first << "," << q.second.first << "," << q.second.second << "," << basebin << "," << div << "," << bin << std::endl;
assert ( high > low );
bool const cross = ((high-1)&mask) != (low & mask);
if ( cross )
{
uint64_t const mid = ((low >> q.first)+1)<<q.first;
// std::cerr << "low=" << low << ",mid=" << mid << ",high=" << high << std::endl;
assert ( mid > low );
assert ( high > mid );
todo.push_back(q_element(q.first-1,upair(low,mid)));
todo.push_back(q_element(q.first-1,upair(mid,high)));
}
else if ( q.first >= 0 )
{
todo.push_back(q_element(q.first-1,upair(low,high)));
}
else
{
// std::cerr << "base " << low << "," << high << " bin " << bin << std::endl;
}
}
void Foam::kineticTheoryModel::solve(const volTensorField& gradUat)
{
if(kineticTheory_)
{
//if (!kineticTheory_)
//{
// return;
//}
//const scalar sqrtPi = sqrt(mathematicalConstant::pi);
if(Berzi_)
{
Info << "Berzi Model is used" << endl;
}
else{
const scalar sqrtPi = sqrt(constant::mathematical::pi);
surfaceScalarField phi = 1.5*rhoa_*phia_*fvc::interpolate(alpha_);
volTensorField dU = gradUat.T();//fvc::grad(Ua_);
volSymmTensorField D = symm(dU);
// NB, drag = K*alpha*beta,
// (the alpha and beta has been extracted from the drag function for
// numerical reasons)
volScalarField Ur = mag(Ua_ - Ub_);
volScalarField betaPrim = alpha_*(1.0 - alpha_)*draga_.K(Ur);
// Calculating the radial distribution function (solid volume fraction is
// limited close to the packing limit, but this needs improvements)
// The solution is higly unstable close to the packing limit.
gs0_ = radialModel_->g0
(
min(max(alpha_, 1e-6), alphaMax_ - 0.01),
alphaMax_
);
// particle pressure - coefficient in front of Theta (Eq. 3.22, p. 45)
volScalarField PsCoeff = granularPressureModel_->granularPressureCoeff
(
alpha_,
gs0_,
rhoa_,
e_
);
// 'thermal' conductivity (Table 3.3, p. 49)
kappa_ = conductivityModel_->kappa(alpha_, Theta_, gs0_, rhoa_, da_, e_);
// particle viscosity (Table 3.2, p.47)
mua_ = viscosityModel_->mua(alpha_, Theta_, gs0_, rhoa_, da_, e_);
dimensionedScalar Tsmall
(
"small",
dimensionSet(0 , 2 ,-2 ,0 , 0, 0, 0),
1.0e-6
);
dimensionedScalar TsmallSqrt = sqrt(Tsmall);
volScalarField ThetaSqrt = sqrt(Theta_);
// dissipation (Eq. 3.24, p.50)
volScalarField gammaCoeff =
12.0*(1.0 - sqr(e_))*sqr(alpha_)*rhoa_*gs0_*(1.0/da_)*ThetaSqrt/sqrtPi;
// Eq. 3.25, p. 50 Js = J1 - J2
volScalarField J1 = 3.0*betaPrim;
volScalarField J2 =
0.25*sqr(betaPrim)*da_*sqr(Ur)
/(max(alpha_, 1e-6)*rhoa_*sqrtPi*(ThetaSqrt + TsmallSqrt));
// bulk viscosity p. 45 (Lun et al. 1984).
lambda_ = (4.0/3.0)*sqr(alpha_)*rhoa_*da_*gs0_*(1.0+e_)*ThetaSqrt/sqrtPi;
// stress tensor, Definitions, Table 3.1, p. 43
volSymmTensorField tau = 2.0*mua_*D + (lambda_ - (2.0/3.0)*mua_)*tr(D)*I;
if (!equilibrium_)
{
// construct the granular temperature equation (Eq. 3.20, p. 44)
// NB. note that there are two typos in Eq. 3.20
// no grad infront of Ps
// wrong sign infront of laplacian
fvScalarMatrix ThetaEqn
(
fvm::ddt(1.5*alpha_*rhoa_, Theta_)
+ fvm::div(phi, Theta_, "div(phi,Theta)")
==
fvm::SuSp(-((PsCoeff*I) && dU), Theta_)
+ (tau && dU)
+ fvm::laplacian(kappa_, Theta_, "laplacian(kappa,Theta)")
+ fvm::Sp(-gammaCoeff, Theta_)
+ fvm::Sp(-J1, Theta_)
+ fvm::Sp(J2/(Theta_ + Tsmall), Theta_)
);
ThetaEqn.relax();
ThetaEqn.solve();
}
else
{
// equilibrium => dissipation == production
// Eq. 4.14, p.82
volScalarField K1 = 2.0*(1.0 + e_)*rhoa_*gs0_;
volScalarField K3 = 0.5*da_*rhoa_*
(
(sqrtPi/(3.0*(3.0-e_)))
*(1.0 + 0.4*(1.0 + e_)*(3.0*e_ - 1.0)*alpha_*gs0_)
+1.6*alpha_*gs0_*(1.0 + e_)/sqrtPi
);
volScalarField K2 =
4.0*da_*rhoa_*(1.0 + e_)*alpha_*gs0_/(3.0*sqrtPi) - 2.0*K3/3.0;
volScalarField K4 = 12.0*(1.0 - sqr(e_))*rhoa_*gs0_/(da_*sqrtPi);
volScalarField trD = tr(D);
volScalarField tr2D = sqr(trD);
volScalarField trD2 = tr(D & D);
volScalarField t1 = K1*alpha_ + rhoa_;
volScalarField l1 = -t1*trD;
volScalarField l2 = sqr(t1)*tr2D;
volScalarField l3 = 4.0*K4*max(alpha_, 1e-6)*(2.0*K3*trD2 + K2*tr2D);
Theta_ = sqr((l1 + sqrt(l2 + l3))/(2.0*(alpha_ + 1.0e-4)*K4));
}
Theta_.max(1.0e-15);
Theta_.min(1.0e+3);
volScalarField pf = frictionalStressModel_->frictionalPressure
(
alpha_,
alphaMinFriction_,
alphaMax_,
Fr_,
eta_,
p_
);
PsCoeff += pf/(Theta_+Tsmall);
PsCoeff.min(1.0e+10);
PsCoeff.max(-1.0e+10);
// update particle pressure
pa_ = PsCoeff*Theta_;
// frictional shear stress, Eq. 3.30, p. 52
volScalarField muf = frictionalStressModel_->muf
(
alpha_,
alphaMax_,
pf,
D,
phi_
);
// add frictional stress
mua_ += muf;
//-AO Inconsistency of equations
const scalar constSMALL = 0.001; //1.e-06;
mua_ /= (fvc::average(alpha_) + scalar(constSMALL));
lambda_ /= (fvc::average(alpha_) + scalar(constSMALL));
//-AO
mua_.min(1.0e+2);
mua_.max(0.0);
Info<< "kinTheory: max(Theta) = " << max(Theta_).value() << endl;
volScalarField ktn = mua_/rhoa_;
Info<< "kinTheory: min(nua) = " << min(ktn).value()
<< ", max(nua) = " << max(ktn).value() << endl;
Info<< "kinTheory: min(pa) = " << min(pa_).value()
<< ", max(pa) = " << max(pa_).value() << endl;
//}
/*
volScalarField& Foam::kineticTheoryModel::ppMagf(const volScalarField& alphaUpdate)
{
volScalarField alpha = alphaUpdate;
gs0_ = radialModel_->g0(min(alpha, alphaMinFriction_), alphaMax_);
gs0Prime_ = radialModel_->g0prime(min(alpha, alphaMinFriction_), alphaMax_);
// Computing ppMagf
ppMagf_ = Theta_*granularPressureModel_->granularPressureCoeffPrime
(
alpha,
gs0_,
gs0Prime_,
rhoa_,
e_
);
volScalarField ppMagfFriction = frictionalStressModel_->frictionalPressurePrime
(
alpha,
alphaMinFriction_,
alphaMax_,
Fr_,
eta_,
p_
);
// NOTE: this might not be appropriate if J&J model is used (verify)
forAll(alpha, cellI)
{
if(alpha[cellI] >= alphaMinFriction_.value())
{
ppMagf_[cellI] = ppMagfFriction[cellI];
}
}
ppMagf_.correctBoundaryConditions();
return ppMagf_;
}
*/}
}
else if(mofidiedKineticTheoryPU_)
{
//if (!mofidiedKineticTheoryPU_)
//{
// return;
//}
Info << " " << endl;
Info << "Modified kinetic theory model - Chialvo-Sundaresan " << endl;
bool testMKTimp(false);
if(kineticTheoryProperties_.found("testMKTimp"))
{
testMKTimp = true;
Info << "Modified kinetic theory model - testing implementation (chi=1,eEff=e, ksi=1) " << endl;
}
bool diluteCorrection(false);
if(kineticTheoryProperties_.found("diluteCorrection"))
{
testMKTimp = false;
diluteCorrection = true;
Info << "Modified kinetic theory model - Only dilute correction " << endl;
}
bool denseCorrection(false);
if(kineticTheoryProperties_.found("denseCorrection"))
{
testMKTimp = false;
diluteCorrection = false;
denseCorrection = true;
Info << "Modified kinetic theory model - Only dense correction " << endl;
}
bool frictionBlending(false);
if(kineticTheoryProperties_.found("frictionBlending"))
{
frictionBlending = true;
Info << "Modified kinetic theory model - Include Friction Blneding " << endl;
}
if(decomposePp_) Info << "Decompose Pp into Pp - PpStar " << endl;
bool verboseMKT(false);
if(kineticTheoryProperties_.found("verboseMKT")) verboseMKT = true;
const scalar Pi = constant::mathematical::pi;
const scalar sqrtPi = sqrt(constant::mathematical::pi);
const scalar constSMALL = 1.e-06; //1.e-06; 1.e-03;
// Read from dictionary
muFric_ = readScalar(kineticTheoryProperties_.lookup("muFriction"));
eEff_ = e_ - 3.0 / 2.0 * muFric_ * exp(-3.0 * muFric_);
// If only test MKT implementation
if(testMKTimp) eEff_ = e_;
alphaf_ = readScalar(kineticTheoryProperties_.lookup("alphaDiluteInertialUpperLimit"));
alphac_ = readScalar(kineticTheoryProperties_.lookup("alphaCritical"));
alphad_ = readScalar(kineticTheoryProperties_.lookup("alphaDelta"));
upsilons_ = readScalar(kineticTheoryProperties_.lookup("yieldStressRatio"));
// Model parameters
dimensionedScalar I0(0.2); // Table 2, p.15
dimensionedScalar const_alpha(0.36); // Table 2, p.15
dimensionedScalar const_alpha1(0.06); // Table 2, p.15
// Calculating the radial distribution function (solid volume fraction is
// limited close to the packing limit, but this needs improvements)
// The solution is higly unstable close to the packing limit.
gs0_ = radialModel_->g0jamming
(
Ua_.mesh(),
//max(alpha, scalar(constSMALL)),
min(max(alpha_, scalar(constSMALL)),alphaMax_ - 0.01), //changed by YG
alphaMax_,
alphad_, ///changed by YG
alphac_
);
// particle pressure - coefficient in front of T (Eq. 1, p. 3)
volScalarField PsCoeff // -> rho_p * H
(
granularPressureModel_->granularPressureCoeff
(
alpha_,
gs0_,
rhoa_,
e_
)
);
PsCoeff.max(1.0e-15);
// PsCoeff.min(1.0e+10);
// PsCoeff.max(-1.0e+10);
// Solid kinetic+collisional viscosity mua_ = nu_k^star + nu_c^star, Eq. 8,9, p.4
// If Garzo-Dufty viscosity is used (viscosity is dimensionless), there is issue with dimension of mu1
mua_ = viscosityModel_->mua(alpha_, Theta_, gs0_, rhoa_, da_, e_);
// Solid bulk viscosity mua_ = nu_k^star + nu_c^star, Eq. 10, p.4
// If Garzo-Dufty viscosity is used (viscosity is dimensionless), there is issue with dimension of mu1
// Create dimensionedScalar
dimensionedScalar viscDim("zero", dimensionSet(1, -1, -1, 0, 0), 1.0);
lambda_ = viscDim * 384.0 / ( 25.0 * Pi ) * ( 1.0 + e_ ) * alpha_ * alpha_ * gs0_ ;
//lambda_ = (4.0/3.0)*sqr(alpha_)*rhoa_*da_*gs0_*(1.0+e_)*sqrt(Theta_)/sqrtPi;
volScalarField ratioBulkShearVisc(lambda_/(mua_+lambda_));
// J Eq.5, p3
volScalarField J_( 5.0 * sqrtPi / 96.0 * ( mua_ + lambda_ ) / viscDim ); // Dimension issue
// K Eq.6, p3
volScalarField K_(12.0/sqrtPi*alpha_*alpha_*gs0_*(1.0-e_*e_));
// K' Eq.26, p8 modified dissipation due to friction
volScalarField Kmod_(K_*(1.0 - eEff_*eEff_)/(1.0 - e_*e_));
// M Eq.30 p.9
volScalarField M_( max( J_ / max( Kmod_, constSMALL) , const_alpha1 / sqrt( max(alphac_ - alpha_, constSMALL) ) ) );
// Shear stress rate tensor
volTensorField dU(gradUat.T());
volSymmTensorField D(symm(dU));
// Shear stress rate (gammaDot)
volScalarField gammaDot(sqrt(2.*magSqr(D)));
dimensionedScalar gammaDotSmall("gammaDotSmall",dimensionSet(0 , 0 , -1 , 0 , 0, 0, 0), constSMALL);
// Dilute inertia temperature Eq.24, p8
volScalarField ThetaDil_ = ( J_ / max ( Kmod_ , 1e-1 ) ) * ( gammaDot * da_ ) * ( gammaDot * da_ );
// Dense inertia temperature Eq.27, p8
// volScalarField ThetaDense_ = const_alpha1 * ( gammaDot * da_ ) * ( gammaDot * da_ )
// / sqrt( max(alphac_ - alpha_, constSMALL) );
volScalarField ThetaDense_ = const_alpha1 * ( gammaDot * da_ ) * ( gammaDot * da_ )
/ sqrt( max(alphac_ - alpha_, alphad_) )
+ max(alpha_ - (alphac_ - alphad_),0.0) * 0.5 *const_alpha1*( gammaDot * da_ ) * ( gammaDot * da_)*pow(alphad_,-1.5);
// Theta
Theta_ = max(ThetaDil_,ThetaDense_) ;
if(testMKTimp || diluteCorrection) Theta_ = ThetaDil_;
if(denseCorrection) Theta_ = ThetaDense_;
// Limit granular temperature
Theta_.max(1.0e-15);
Theta_.min(1.0e+3);
// Particle pressure
pa_ = PsCoeff * Theta_;
if(frictionBlending)
{
/* volScalarField pf = frictionalStressModel_->frictionalPressure
(
alpha_,
alphaMinFriction_-0.001,
alphaMax_,
Fr_,
eta_,
p_
);
pa_ = pa_ + pf;
*/
// pa_ =pa_ + dimensionedScalar("1e24", dimensionSet(1, -1, -2, 0, 0), Fr_.value())*pow(max(alpha_ - (alphaMinFriction_), scalar(0)), 2/3);
pa_ =pa_ + dimensionedScalar("5810", dimensionSet(1, 0, -2, 0, 0), 581.0)/da_*pow(max(alpha_ - (alphaMinFriction_-0.0), scalar(0)), 2.0/3.0);
// pa_ =pa_ + dimensionedScalar("4.7e9", dimensionSet(1, -1, -2, 0, 0), 4.7e9)*pow(max(alpha_ - (alphaMinFriction_-0.0), scalar(0)), 1.56);
// forAll(alpha_, cellI)
// {
// if(alpha_[cellI] >= (alphaMinFriction_.value()-0.00001))
// {
// pa_[cellI] = pa_[cellI] + 581.0/da_.value()*pow(alpha_[cellI] - (alphaMinFriction_.value()-0.00001), 2.0/3.0);
// }
// }
}
// Psi Eq.32, p.12
dimensionedScalar psi(1.0 + 3.0/10.0*pow((1.0-e_*e_),-1.5)*(1.0-exp(-8.0*muFric_)));
if(testMKTimp) psi = 1.0;
// Shear stress ratio in dilute regime, Eq.33, p.12
dimensionedScalar paSmall("paSmall",dimensionSet(1, -1, -2, 0, 0), constSMALL);
volScalarField inertiaNumber( gammaDot * da_ / sqrt( (pa_ + paSmall) / rhoa_ ) );
// Modified inertia number Eq.35, p.13
volScalarField modInertiaNumber( inertiaNumber / max( alpha_, constSMALL ) );
// Model parameters
volScalarField chi( 1.0 / ( pow( I0 / max( modInertiaNumber,constSMALL ) , 1.5 ) + 1.0 ));
if(testMKTimp || diluteCorrection) chi = max( modInertiaNumber,constSMALL ) / max( modInertiaNumber,constSMALL ) ;
if(denseCorrection) chi= modInertiaNumber - modInertiaNumber;
// Beta + Sigma_tau Eq.49 p.14
volScalarField beta(alpha_ * psi * J_ * sqrt( K_ /( max ( (Kmod_ * ( PsCoeff / rhoa_)), constSMALL ) ) ) );
volScalarField sigmaTau( const_alpha / max( beta, constSMALL ) + ( 1 - const_alpha / max( beta, constSMALL ) ) * chi);
// Sigma_gamma Eq.51 p.14
volScalarField sigmaGamma( beta * sqrt( PsCoeff/rhoa_ ) / max( ( Kmod_ * M_ ), constSMALL ) * sigmaTau);
// dissipation
volScalarField gammaCoeff
(
// van Wachem (Eq. 3.24, p.50) 12.0*(1.0 - sqr(e_))*sqr(alpha_)*rhoa_*gs0_*(1.0/da_)*ThetaSqrt/sqrtPi
// Chialvo & Sundaresan Eq.50 p.14
//rhoa_ / da_ * Kmod_ * Theta_ * sqrt(Theta_) * sigmaGamma
rhoa_ / da_ * Kmod_ * sqrt(Theta_) * sigmaGamma
);
// Blending function
volScalarField func_B( const_alpha + ( beta-const_alpha ) * chi );
// Shear stress ratio
upsilon_ = upsilons_ * (1 - chi) + func_B * modInertiaNumber;
// Shear stress
volSymmTensorField S( D - 1./3.*tr(D)*I );
volSymmTensorField hatS( 2. * S / max( gammaDot, gammaDotSmall ) );
// Shear stress based on pressure and ratio
tau_ = pa_ * upsilon_ * hatS;
// Viscosity
mua_ = ( pa_ * upsilon_ ) / (max( gammaDot, gammaDotSmall )) ;
// Divide by alpha (to be consistent with OpenFOAM implementation)
/* mua_ /= (fvc::average(alpha_) + scalar(0.001));
tau_ /= (fvc::average(alpha_) + scalar(0.001));
lambda_ /= (fvc::average(alpha_) + scalar(0.001)); */
mua_ /= max(alpha_, scalar(constSMALL));
// Limit mua
mua_.min(3e+02);
mua_.max(0.0);
// Limit lambda
lambda_ = mua_ * ratioBulkShearVisc;
// Limit shear stress
tau_ = mua_ * gammaDot * hatS;
// tau_ /= max(alpha_, scalar(constSMALL));
// lambda_ /= max(alpha_, scalar(constSMALL));
//mua_ /= max(alpha_, scalar(constSMALL));
//tau_ /= max(alpha_, scalar(constSMALL));
//lambda_ /= max(alpha_, scalar(constSMALL));
if(verboseMKT)
{
#include "verboseMKT.H"
}
//-AO, YG - Decompose particle pressure, Sundar's idea
if(decomposePp_)
{
pa_ /= (fvc::average(alpha_) + scalar(0.001));
//pa_.correctBoundaryConditions();
pa_.max(1.0e-15);
}
}
else
{
return;
}
}
double sigmaiN(double T, double M, double k0, double k)
{
return GW*T*T/(8*PI*k)*(k0/k*I1(k0/T,k/T)-M*M/(2*k*T)*I0(k0/T,k/T))*k/k;
}
