void L2Sphere::Projection(Variable *x, Vector *v, Vector *result) const
{
const double *xl = x->ObtainReadData();
double nume = Metric(x, x, v);
scalarVectorAddVector(x, -nume, x, v, result);
};
void Stiefel::DiffqfRetraction(Variable *x, Vector *etax, Variable *y, Vector *xix, Vector *result, bool IsEtaXiSameDir) const
{
Vector *extempx = EMPTYEXTR->ConstructEmpty();
const double *extempxTV;
if (IsIntrApproach)
{
ObtainExtr(x, xix, extempx);
extempxTV = extempx->ObtainReadData();
}
else
{
xix->CopyTo(extempx);
extempxTV = extempx->ObtainWritePartialData();
}
const double *yM = y->ObtainReadData();
double *resultTV = result->ObtainWriteEntireData();
const SharedSpace *HHR = y->ObtainReadTempData("HHR");
const double *ptrHHR = HHR->ObtainReadData();
double *YtVRinv = new double[p * p];
integer inc = 1, N = n, P = p;
char *left = const_cast<char *> ("r"), *up = const_cast<char *> ("u"),
*transn = const_cast<char *> ("n"), *transt = const_cast<char *> ("t"), *nonunit = const_cast<char *> ("n");
double one = 1, zero = 0;
dtrsm_(left, up, transn, nonunit, &N, &P, &one, const_cast<double *> (ptrHHR), &N, const_cast<double *> (extempxTV), &N);
double sign;
for (integer i = 0; i < P; i++)
{
sign = (ptrHHR[i + i * N] >= 0) ? 1 : -1;
dscal_(&N, &sign, const_cast<double *> (extempxTV + i * N), &inc);
}
dgemm_(transt, transn, &P, &P, &N, &one, const_cast<double *> (yM), &N, const_cast<double *> (extempxTV), &N, &zero, YtVRinv, &P);
for (integer i = 0; i < p; i++)
{
YtVRinv[i + p * i] = -YtVRinv[i + p * i];
for (integer j = i + 1; j < p; j++)
{
YtVRinv[i + p * j] = -YtVRinv[j + p * i] - YtVRinv[i + p * j];
YtVRinv[j + p * i] = 0;
}
}
dgemm_(transn, transn, &N, &P, &P, &one, const_cast<double *> (yM), &N, YtVRinv, &P, &one, const_cast<double *> (extempxTV), &N);
if (IsIntrApproach)
{
ObtainIntr(y, extempx, result);
}
else
{
extempx->CopyTo(result);
}
delete[] YtVRinv;
delete extempx;
if (IsEtaXiSameDir && (HasHHR || UpdBetaAlone))
{
const double *etaxTV = etax->ObtainReadData();
const double *xixTV = xix->ObtainReadData();
double EtatoXi = sqrt(Metric(x, etax, etax) / Metric(x, xix, xix));
SharedSpace *beta = new SharedSpace(1, 3);
double *betav = beta->ObtainWriteEntireData();
betav[0] = sqrt(Metric(x, etax, etax) / Metric(x, result, result)) / EtatoXi;
betav[1] = Metric(x, etax, etax);
betav[2] = Metric(x, result, result) * EtatoXi * EtatoXi;
etax->AddToTempData("beta", beta);
if (HasHHR)
{
Vector *TReta = result->ConstructEmpty();
result->CopyTo(TReta);
ScaleTimesVector(x, betav[0] * EtatoXi, TReta, TReta);
SharedSpace *SharedTReta = new SharedSpace(TReta);
etax->AddToTempData("betaTReta", SharedTReta);
}
}
};
bool CCoordTransService::Metric (const Punkt &Pt, double *pX, double *pY)
{
return Metric (Pt.GetX(), Pt.GetY(), pX, pY);
}
/*
Triangles::BThBoundary(Edge e,Real 8) const
{
// pointeur of the background must be on
//
Edge be = e.on;
}
*/
int Triangles::SplitElement(int choice)
{
Direction NoDirOfSearch;
const int withBackground = &BTh != this && &BTh;
if (verbosity>2)
cout << " -- SplitElement " << (choice? " Q->4Q and T->4T " : " Q->4Q or T->3Q " ) << endl;;
if (verbosity>5)
cout << endl << " (in) Nb of Quadrilaterals = " << NbOfQuad
<< " Nb Of Triangles = " << nbt-NbOutT- NbOfQuad*2
<< " Nb of outside triangles = " << NbOutT << endl;
ReNumberingTheTriangleBySubDomain();
#ifdef DRAWING2
Draw();
inquire();
#endif
int nswap =0;
const Int4 nfortria( choice ? 4 : 6);
if(withBackground)
{
BTh.SetVertexFieldOn();
SetVertexFieldOnBTh();
}
else
BTh.SetVertexFieldOn();
Int4 newnbt=0,newnbv=0;
Int4 * kedge = 0;
Int4 newNbOfQuad=NbOfQuad;
Int4 * ksplit= 0, * ksplitarray=0;
Int4 kkk=0;
int ret =0;
if (nbvx<nbv+nbe) return 1;//
Triangles * OCurrentTh= CurrentTh;
CurrentTh = this;
// 1) create the new points by spliting the internal edges
// set the
Int4 nbvold = nbv;
Int4 nbtold = nbt;
Int4 NbOutTold = NbOutT;
Int4 NbEdgeOnGeom=0;
Int4 i;
nbt = nbt - NbOutT; // remove all the the ouside triangles
Int4 nbtsave = nbt;
Triangle * lastT = triangles + nbt;
for (i=0;i<nbe;i++)
if(edges[i].on) NbEdgeOnGeom++;
Int4 newnbe=nbe+nbe;
// Int4 newNbVerticesOnGeomVertex=NbVerticesOnGeomVertex;
Int4 newNbVerticesOnGeomEdge=NbVerticesOnGeomEdge+NbEdgeOnGeom;
// Int4 newNbVertexOnBThVertex=NbVertexOnBThVertex;
Int4 newNbVertexOnBThEdge=withBackground ? NbVertexOnBThEdge+NbEdgeOnGeom:0;
// do allocation for pointeur to the geometry and background
VertexOnGeom * newVerticesOnGeomEdge = new VertexOnGeom[newNbVerticesOnGeomEdge];
VertexOnEdge *newVertexOnBThEdge = newNbVertexOnBThEdge ? new VertexOnEdge[newNbVertexOnBThEdge]:0;
if (NbVerticesOnGeomEdge)
memcpy(newVerticesOnGeomEdge,VerticesOnGeomEdge,sizeof(VertexOnGeom)*NbVerticesOnGeomEdge);
if (NbVertexOnBThEdge)
memcpy(newVertexOnBThEdge,VertexOnBThEdge,sizeof(VertexOnEdge)*NbVertexOnBThEdge);
Edge *newedges = new Edge [newnbe];
// memcpy(newedges,edges,sizeof(Edge)*nbe);
SetOfEdges4 * edge4= new SetOfEdges4(nbe,nbv);
#ifdef DEBUG
for (i=0;i<nbt;i++)
triangles[i].check();
#endif
#ifdef DRAWING2
reffecran();
#endif
Int4 k=nbv;
Int4 kk=0;
Int4 kvb = NbVertexOnBThEdge;
Int4 kvg = NbVerticesOnGeomEdge;
Int4 ie =0;
Edge ** edgesGtoB=0;
if (withBackground)
edgesGtoB= BTh.MakeGeometricalEdgeToEdge();
Int4 kerr=0,ferr=0;
for (i=0;i<nbe;i++)
newedges[ie].on=0;
for (i=0;i<nbe;i++)
{
GeometricalEdge *ong = edges[i].on;
newedges[ie]=edges[i];
newedges[ie].adj[0]=newedges+(edges[i].adj[0]-edges) ;
newedges[ie].adj[1]=newedges + ie +1;
R2 A = edges[i][0],B = edges[i][1];
// cout << " ie = " << ie <<" v0 = " << Number(newedges[ie][0]) << endl;
kk += (i == edge4->addtrie(Number(edges[i][0]),Number(edges[i][1])));
if (ong) // a geometrical edges
{
if (withBackground)
{
// walk on back ground mesh
// newVertexOnBThEdge[ibe++] = VertexOnEdge(vertices[k],bedge,absicsseonBedge);
// a faire -- difficile
// the first PB is to now a background edge between the 2 vertices
assert(edgesGtoB);
// cout << " ie = " << ie <<" v0 = " << Number(newedges[ie][0]) << endl;
ong= ProjectOnCurve(*edgesGtoB[Gh.Number(edges[i].on)],
edges[i][0],edges[i][1],0.5,vertices[k],
newVertexOnBThEdge[kvb],
newVerticesOnGeomEdge[kvg++]);
vertices[k].ReferenceNumber= edges[i].ref;
vertices[k].DirOfSearch = NoDirOfSearch;
;
// get the Info on background mesh
Real8 s = newVertexOnBThEdge[kvb];
Vertex & bv0 = newVertexOnBThEdge[kvb][0];
Vertex & bv1 = newVertexOnBThEdge[kvb][1];
// compute the metrix of the new points
vertices[k].m = Metric(1-s,bv0,s,bv1);
kvb++;
// cout << " ie = " << ie <<" v0 = " << Number(newedges[ie][0]) << endl;
}
else
{
ong=Gh.ProjectOnCurve(edges[i],
0.5,vertices[k],newVerticesOnGeomEdge[kvg++]);
// vertices[k].i = toI2( vertices[k].r);
vertices[k].ReferenceNumber = edges[i].ref;
vertices[k].DirOfSearch = NoDirOfSearch;
vertices[k].m = Metric(0.5,edges[i][0],0.5,edges[i][1]);
}
}
else // straigth line edge ---
{
vertices[k].r = ((R2) edges[i][0] + (R2) edges[i][1] )*0.5;
vertices[k].m = Metric(0.5,edges[i][0],0.5,edges[i][1]);
vertices[k].on = 0;
}
//vertices[k].i = toI2( vertices[k].r);
R2 AB = vertices[k].r;
R2 AA = (A+AB)*0.5;
R2 BB = (AB+B)*0.5;
vertices[k].ReferenceNumber = edges[i].ref;
vertices[k].DirOfSearch = NoDirOfSearch;
newedges[ie].on = Gh.Contening(AA,ong);
newedges[ie++].v[1]=vertices+k;
newedges[ie]=edges[i];
newedges[ie].adj[0]=newedges + ie -1;
newedges[ie].adj[1]=newedges+(edges[i].adj[1]-edges) ;
newedges[ie].on = Gh.Contening(BB,ong);
newedges[ie++].v[0]=vertices+k;
// cout << " ie = " << ie-2 << " vm " << k << " v0 = " << Number(newedges[ie-2][0])
// << " v1 = " << Number(newedges[ie-1][1])
// << " ong =" << ong-Gh.edges
// << " on 0 =" << newedges[ie-2].on -Gh.edges << AA
// << " on 1 =" << newedges[ie-1].on -Gh.edges << BB
// << endl;
k++;
}
#ifdef DEBUG
assert(kvb == newNbVertexOnBThEdge);
// verif edge
{ Vertex *v0 = vertices, *v1 = vertices+ k;
for (Int4 i=0;i<ie;i++)
{
assert( &newedges[i][0] >= v0 && &newedges[i][0] < v1);
assert( &newedges[i][1] >= v0 && &newedges[i][1] < v1);
}
}
#endif
if (edgesGtoB) delete [] edgesGtoB;
edgesGtoB=0;
newnbv=k;
newNbVerticesOnGeomEdge=kvg;
if (newnbv> nbvx) goto Error;// bug
nbv = k;
kedge = new Int4[3*nbt+1];
ksplitarray = new Int4[nbt+1];
ksplit = ksplitarray +1; // because ksplit[-1] == ksplitarray[0]
for (i=0;i<3*nbt;i++)
kedge[i]=-1;
//
for (i=0;i<nbt;i++)
{
Triangle & t = triangles[i];
assert(t.link);
for(int j=0;j<3;j++)
{
const TriangleAdjacent ta = t.Adj(j);
const Triangle & tt = ta;
if (&tt >= lastT)
t.SetAdj2(j,0,0);// unset adj
const Vertex & v0 = t[VerticesOfTriangularEdge[j][0]];
const Vertex & v1 = t[VerticesOfTriangularEdge[j][1]];
Int4 ke =edge4->findtrie(Number(v0),Number(v1));
if (ke>0)
{
Int4 ii = Number(tt);
int jj = ta;
Int4 ks = ke + nbvold;
kedge[3*i+j] = ks;
if (ii<nbt) // good triangle
kedge[3*ii+jj] = ks;
Vertex &A=vertices[ks];
Real8 aa,bb,cc,dd;
if ((dd=Area2(v0.r,v1.r,A.r)) >=0)
{ // warning PB roundoff error
if (t.link && ( (aa=Area2( A.r , t[1].r , t[2].r )) < 0.0
|| (bb=Area2( t[0].r , A.r , t[2].r )) < 0.0
|| (cc=Area2( t[0].r , t[1].r , A.r )) < 0.0))
ferr++, cerr << " Error : " << ke + nbvold << " not in triangle "
<< i << " In=" << !!t.link
<< " " << aa << " " << bb << " " << cc << " " << dd << endl;
}
else
{
if (tt.link && ( (aa=Area2( A.r , tt[1].r , tt[2].r )) < 0
|| (bb=Area2( tt[0].r , A.r , tt[2].r )) < 0
|| (cc=Area2( tt[0].r , tt[1].r , A.r )) < 0))
ferr++, cerr << " Warning : " << ke + nbvold << " not in triangle " << ii
<< " In=" << !!tt.link
<< " " << aa << " " << bb << " " << cc << " " << dd << endl;
}
}
}
}
if(ferr)
{
cerr << " Number of triangles with P2 interpolation Probleme " << ferr << endl;;
MeshError(9);
}
for (i=0;i<nbt;i++)
{
ksplit[i]=1; // no split by default
const Triangle & t = triangles[ i];
// cout << " Triangle " << i << " " << t << !!t.link << ":: " ;
int nbsplitedge =0;
int nbinvisible =0;
int invisibleedge=0;
int kkk[3];
for (int j=0;j<3;j++)
{
if (t.Hidden(j)) invisibleedge=j,nbinvisible++;
const TriangleAdjacent ta = t.Adj(j);
const Triangle & tt = ta;
const Vertex & v0 = t[VerticesOfTriangularEdge[j][0]];
const Vertex & v1 = t[VerticesOfTriangularEdge[j][1]];
// cout << " ke = " << kedge[3*i+j] << " " << Number(v0) << " " << Number(v1) << "/ ";
if ( kedge[3*i+j] < 0)
{
Int4 ke =edge4->findtrie(Number(v0),Number(v1));
// cout << ":" << ke << "," << !!t.link << " " << &tt ;
if (ke<0) // new
{
if (&tt) // internal triangles all the boundary
{ // new internal edges
Int4 ii = Number(tt);
int jj = ta;
kedge[3*i+j]=k;// save the vertex number
kedge[3*ii+jj]=k;
if (k<nbvx)
{
vertices[k].r = ((R2) v0+(R2) v1 )/2;
//vertices[k].i = toI2( vertices[k].r);
vertices[k].ReferenceNumber=0;
vertices[k].DirOfSearch =NoDirOfSearch;
vertices[k].m = Metric(0.5,v0,0.5,v1);
}
k++;
kkk[nbsplitedge++]=j;
} // tt
else
cerr <<endl << " Bug " <<i<< " " << j << " t=" << t << endl;
} // ke<0
else
{ // ke >=0
kedge[3*i+j]=nbvold+ke;
kkk[nbsplitedge++]=j;// previously splited
}
}
else
kkk[nbsplitedge++]=j;// previously splited
}
assert (nbinvisible<2);
// cout << " " << nbinvisible << " " << nbsplitedge << endl;
switch (nbsplitedge) {
case 0: ksplit[i]=10; newnbt++; break; // nosplit
case 1: ksplit[i]=20+kkk[0];newnbt += 2; break; // split in 2
case 2: ksplit[i]=30+3-kkk[0]-kkk[1];newnbt += 3; break; // split in 3
case 3:
if (nbinvisible) ksplit[i]=40+invisibleedge,newnbt += 4;
else ksplit[i]=10*nfortria,newnbt+=nfortria;
break;
}
assert(ksplit[i]>=40);
}
// now do the element split
newNbOfQuad = 4*NbOfQuad;
nbv = k;
#ifdef DRAWING2
inquire();
#endif
// cout << " Nbv = " << nbv << endl;
kkk = nbt;
ksplit[-1] = nbt;
// look on old true triangles
for (i=0;i<nbtsave;i++)
{
// cout << "Triangle " << i << " " << ksplit[i] << ":" << triangles[i]
// << " ----------------------------------------------- " <<endl;
Triangle * tc=0;
int nbmkadj=0;
Int4 mkadj [100];
mkadj[0]=i;
Int4 kk=ksplit[i]/10;
int ke=(int) (ksplit[i]%10);
assert(kk<7 && kk >0);
// def the numbering k (edge) i vertex
int k0 = ke;
int k1 = NextEdge[k0];
int k2 = PreviousEdge[k0];
int i0 = OppositeVertex[k0];
int i1 = OppositeVertex[k1];
int i2 = OppositeVertex[k2];
Triangle &t0=triangles[i];
Vertex * v0=t0(i0);
Vertex * v1=t0(i1);
Vertex * v2=t0(i2);
// cout << "nbmkadj " << nbmkadj << " it=" << i <<endl;
assert(nbmkadj< 10);
// --------------------------
TriangleAdjacent ta0(t0.Adj(i0)),ta1(t0.Adj(i1)),ta2(t0.Adj(i2));
// save the flag Hidden
int hid[]={t0.Hidden(0),t0.Hidden(1),t0.Hidden(2)};
// un set all adj -- save Hidden flag --
t0.SetAdj2(0,0,hid[0]);
t0.SetAdj2(1,0,hid[1]);
t0.SetAdj2(2,0,hid[2]);
// -- remake
switch (kk) {
case 1: break;// nothing
case 2: //
{
Triangle &t1=triangles[kkk++];
t1=t0;
assert (kedge[3*i+i0]>=0);
Vertex * v3 = vertices + kedge[3*i+k0];
t0(i2) = v3;
t1(i1) = v3;
t0.SetAllFlag(k2,0);
t1.SetAllFlag(k1,0);
}
break;
case 3: //
{
Triangle &t1=triangles[kkk++];
Triangle &t2=triangles[kkk++];
t2=t1=t0;
assert (kedge[3*i+k1]>=0);
assert (kedge[3*i+k2]>=0);
Vertex * v01 = vertices + kedge[3*i+k2];
Vertex * v02 = vertices + kedge[3*i+k1];
t0(i1) = v01;
t0(i2) = v02;
t1(i2) = v02;
t1(i0) = v01;
t2(i0) = v02;
t0.SetAllFlag(k0,0);
t1.SetAllFlag(k1,0);
t1.SetAllFlag(k0,0);
t2.SetAllFlag(k2,0);
}
break;
case 4: //
case 6: // split in 4
{
Triangle &t1=triangles[kkk++];
Triangle &t2=triangles[kkk++];
Triangle &t3=triangles[kkk++];
t3=t2=t1=t0;
assert(kedge[3*i+k0] >=0 && kedge[3*i+k1] >=0 && kedge[3*i+k2] >=0);
Vertex * v12 = vertices + kedge[3*i+k0];
Vertex * v02 = vertices + kedge[3*i+k1];
Vertex * v01 = vertices + kedge[3*i+k2];
// cout << Number(t0(i0)) << " " << Number(t0(i1))
// << " " << Number(t0(i2))
// << " " << kedge[3*i+k0]
// << " " << kedge[3*i+k1]
// << " " << kedge[3*i+k2] << endl;
t0(i1) = v01;
t0(i2) = v02;
t0.SetAllFlag(k0,hid[k0]);
t1(i0) = v01;
t1(i2) = v12;
t0.SetAllFlag(k1,hid[k1]);
t2(i0) = v02;
t2(i1) = v12;
t2.SetAllFlag(k2,hid[k2]);
t3(i0) = v12;
t3(i1) = v02;
t3(i2) = v01;
t3.SetAllFlag(0,hid[0]);
t3.SetAllFlag(1,hid[1]);
t3.SetAllFlag(2,hid[2]);
if ( kk == 6)
{
Triangle &t4=triangles[kkk++];
Triangle &t5=triangles[kkk++];
t4 = t3;
t5 = t3;
t0.SetHidden(k0);
t1.SetHidden(k1);
t2.SetHidden(k2);
t3.SetHidden(0);
t4.SetHidden(1);
t5.SetHidden(2);
if (nbv < nbvx )
{
vertices[nbv].r = ((R2) *v01 + (R2) *v12 + (R2) *v02 ) / 3.0;
vertices[nbv].ReferenceNumber =0;
vertices[nbv].DirOfSearch =NoDirOfSearch;
//vertices[nbv].i = toI2(vertices[nbv].r);
Real8 a3[]={1./3.,1./3.,1./3.};
vertices[nbv].m = Metric(a3,v0->m,v1->m,v2->m);
Vertex * vc = vertices +nbv++;
t3(i0) = vc;
t4(i1) = vc;
t5(i2) = vc;
}
else
goto Error;
}
}
break;
}
// cout << " -- " << i << " " << nbmkadj << " " << kkk << " " << tc << endl;
// t0.SetDetf();
// save all the new triangles
mkadj[nbmkadj++]=i;
Int4 jj;
if (t0.link)
for (jj=nbt;jj<kkk;jj++)
{
triangles[jj].link=t0.link;
t0.link= triangles+jj;
mkadj[nbmkadj++]=jj;
// triangles[jj].SetDet();
}
// cout << " -- " << i << " " << nbmkadj << endl;
assert(nbmkadj<=13);// 13 = 6 + 4 + 3
if (kk==6) newNbOfQuad+=3;
// triangles[i].Draw();
for (jj=ksplit[i-1];jj<kkk;jj++)
// triangles[jj].SetDet();
// triangles[jj].Draw();
nbt = kkk;
ksplit[i]= nbt; // save last adresse of the new triangles
kkk = nbt;
}
// cout << " nv = " << nbv << " nbt = " << nbt << endl;
for (i=0;i<nbv;i++)
vertices[i].m = vertices[i].m*2.;
//
if(withBackground)
for (i=0;i<BTh.nbv;i++)
BTh.vertices[i].m = BTh.vertices[i].m*2.;
#ifdef DRAWING2
Draw();
inquire();
#endif
ret = 2;
if (nbt>= nbtx) goto Error; // bug
if (nbv>= nbvx) goto Error; // bug
// generation of the new triangles
SetIntCoor("In SplitElement");
ReMakeTriangleContainingTheVertex();
if(withBackground)
BTh.ReMakeTriangleContainingTheVertex();
delete [] edges;
edges = newedges;
nbe = newnbe;
NbOfQuad = newNbOfQuad;
for (i=0;i<NbSubDomains;i++)
{
Int4 k = subdomains[i].edge- edges;
subdomains[i].edge = edges+2*k; // spilt all edge in 2
}
if (ksplitarray) delete [] ksplitarray;
if (kedge) delete [] kedge;
if (edge4) delete edge4;
if (VerticesOnGeomEdge) delete [] VerticesOnGeomEdge;
VerticesOnGeomEdge= newVerticesOnGeomEdge;
if(VertexOnBThEdge) delete [] VertexOnBThEdge;
VertexOnBThEdge = newVertexOnBThEdge;
NbVerticesOnGeomEdge = newNbVerticesOnGeomEdge;
NbVertexOnBThEdge=newNbVertexOnBThEdge;
// ReMakeTriangleContainingTheVertex();
FillHoleInMesh();
#ifdef DEBUG
for (i=0;i<nbt;i++)
triangles[i].check();
#endif
if (verbosity>2)
cout << " (out) Nb of Quadrilaterals = " << NbOfQuad
<< " Nb Of Triangles = " << nbt-NbOutT- NbOfQuad*2
<< " Nb of outside triangles = " << NbOutT << endl;
CurrentTh=OCurrentTh;
return 0; //ok
Error:
nbv = nbvold;
nbt = nbtold;
NbOutT = NbOutTold;
// cleaning memory ---
delete newedges;
if (ksplitarray) delete [] ksplitarray;
if (kedge) delete [] kedge;
if (newVerticesOnGeomEdge) delete [] newVerticesOnGeomEdge;
if (edge4) delete edge4;
if(newVertexOnBThEdge) delete [] newVertexOnBThEdge;
CurrentTh= OCurrentTh;
return ret; // ok
}
cv::Mat
WavefrontSensor::WavefrontSensing(const std::vector<cv::Mat>& d, const double& meanPowerNoise)
{
unsigned int numberOfZernikes = 20; //total number of zernikes to be considered
int M = numberOfZernikes;
int K = d.size();
cv::Mat Q2;
//We introduce here the lineal relationship between parameter phases of each optical path
partlyKnownDifferencesInPhaseConstraints(M, K, Q2);
std::vector<cv::Mat> Q2_v = {Q2, cv::Mat::zeros(Q2.size(), Q2.type())};
cv::Mat LEC; //Linear equality constraints
cv::merge(Q2_v, LEC); //Build also the complex version of Q2
//process each patch independently
cv::Mat dd;
std::vector<cv::Mat> d_w;
std::vector<Metric> mtrc_v;
std::vector<std::pair<cv::Range,cv::Range> > rngs;
unsigned int pixelsBetweenTiles = (int)(d.front().cols);
unsigned int tileSize = 34;
OpticalSetup tsettings( tileSize );
std::shared_ptr<Zernike> zrnk = std::make_shared<Zernike>(tsettings.pupilRadiousPixels(), tileSize, numberOfZernikes);
divideIntoTiles(d.front().size(), pixelsBetweenTiles, tileSize, rngs);
//Random row selector: Pick incoherent measurements
cv::Mat eye_nn = cv::Mat::eye(K*tileSize*tileSize, K*tileSize*tileSize, cv::DataType<double>::type);
unsigned int a = 400; //number of incoheren measurements
cv::Mat shuffle_eye;
shuffleRows(eye_nn, shuffle_eye);
//Split 'a' into rngs.size() pieces
std::vector<cv::Mat> A_v = {shuffle_eye(cv::Range(0, a), cv::Range::all()), cv::Mat::zeros(a, K*tileSize*tileSize, cv::DataType<double>::type)};
cv::Mat A;
cv::merge(A_v, A);
std::cout << "Number of anisoplanatic patches to annalize at once: " << rngs.size() << std::endl;
for(auto rng_i : rngs)
{
cv::Mat d_col;
//get ready dataset format
std::vector<cv::Mat> D;
std::vector<cv::Mat> d_col_v;
for(cv::Mat di : d)
{
cv::Mat Di;
cv::dft(di(rng_i.first, rng_i.second), Di, cv::DFT_COMPLEX_OUTPUT + cv::DFT_SCALE);
fftShift(Di);
D.push_back(Di);
cv::Mat Di_t(Di.t());
d_col_v.push_back(Di_t.reshape(0, Di_t.total() ));
}
cv::vconcat(d_col_v, d_col);
cv::gemm(A, d_col, 1.0, cv::Mat(), 1.0, d_col); //Picks rows randomly
d_w.push_back( d_col );
mtrc_v.push_back( Metric(D, zrnk, meanPowerNoise) );
}
cv::vconcat(d_w, dd);
//-----------------------BY MEANS OF CONVEX OPTIMIZATION:
//Objective function and gradient of the objective function
if(false)
{
for(auto mtrc : mtrc_v)
{
std::function<double(cv::Mat)> func = std::bind(&Metric::objective, &mtrc, std::placeholders::_1);
std::function<cv::Mat(cv::Mat)> dfunc = std::bind(&Metric::gradient, &mtrc, std::placeholders::_1);
ConvexOptimization minimizationKit;
cv::Mat x0_conv = cv::Mat::zeros(M*K, 1, cv::DataType<double>::type); //reset starting point
//Lambda function that turn minimize function + constraints problem into minimize function lower dimension problem
auto F_constrained = [] (cv::Mat x, std::function<double(cv::Mat)> func, const cv::Mat& Q2) -> double
{
return func(Q2*x);
};
auto DF_constrained = [] (cv::Mat x, std::function<cv::Mat(cv::Mat)> dfunc, const cv::Mat& Q2) -> cv::Mat
{
return Q2.t() * dfunc(Q2*x);
};
std::function<double(cv::Mat)> f_constrained = std::bind(F_constrained, std::placeholders::_1, func, Q2);
std::function<cv::Mat(cv::Mat)> df_constrained = std::bind(DF_constrained, std::placeholders::_1, dfunc, Q2);
//Define a new starting point with lower dimensions after reduction with contraints
cv::Mat p_constrained = Q2.t() * x0_conv;
ConvexOptimization min;
min.perform_BFGS(p_constrained, f_constrained, df_constrained);
x0_conv = Q2 * p_constrained; //Go back to original dimensional
std::cout << "mimumum: " << x0_conv.t() << std::endl;
}
std::cout << "END OF CONVEX OPTIMIZATION" << std::endl;
}
//-----------------------BY MEANS OF SPARSE RECOVERY:
//Create phase_div bias: only for the case of two diversity images!!
// cv::Mat phase_div = cv::Mat::zeros(rngs.size()*M*K, 1, cv::DataType<double>::type);
// phase_div.at<double>(M + 3, 0) = tsettings.k() * 3.141592/(2.0*std::sqrt(3.0));
cv::Mat x0 = cv::Mat::zeros(rngs.size()*M*K, 1, cv::DataType<double>::type); //Starting point
std::vector<double> gamma_v(M*K, 1.0);
for(unsigned int count=0;count<600;++count)
{
std::vector<cv::Mat> x0_vvv;
cv::split(x0, x0_vvv);
x0_vvv.at(0).copyTo(x0);
cv::Mat_<std::complex<double> > blockMatrix_M;
std::vector<cv::Mat> De_v;
for(unsigned int t=0; t < rngs.size(); ++t)
{
cv::Mat jacob_i;
mtrc_v.at(t).jacobian( x0(cv::Range(t*M*K, (t*M*K) + (M*K)), cv::Range::all()), jacob_i );
cv::gemm(A, jacob_i, 1.0, cv::Mat(), 1.0, jacob_i); //Picks rows randomly
cv::gemm(jacob_i, LEC, 1.0, cv::Mat(), 1.0, jacob_i); //Apply constraints LECs
cv::copyMakeBorder(blockMatrix_M, blockMatrix_M, 0, jacob_i.size().height, 0, jacob_i.size().width, cv::BORDER_CONSTANT, cv::Scalar(0.0, 0.0) );
cv::Rect rect(cv::Point(t*jacob_i.size().width, t*jacob_i.size().height), jacob_i.size() );
jacob_i.copyTo(blockMatrix_M( rect ));
cv::Mat De_i;
mtrc_v.at(t).phi( x0(cv::Range(t*M*K, (t*M*K) + (M*K)), cv::Range::all()), De_i );
cv::gemm(A, De_i, 1.0, cv::Mat(), 1.0, De_i); //Picks rows randomly
De_v.push_back( De_i );
}
cv::Mat De;
cv::vconcat(De_v, De);
std::vector<cv::Mat> x0_v = {x0, cv::Mat::zeros(x0.size(), x0.type())};
cv::merge(x0_v, x0);
//Apply algorithm to get solution
unsigned int blkLen = rngs.size();
cv::Mat blockMatrix_M_r;
reorderColumns(blockMatrix_M, M, blockMatrix_M_r); //reorder columns so correlated data form a single block
gamma_v = std::vector<double>(M*K, 1.0);
//cv::Mat coeffs = perform_BSBL(blockMatrix_M_r, dd - De, NoiseLevel::Noiseless, gamma_v, blkLen); //Noiseless, LittleNoise
//cv::Mat coeffs = perform_SBL(blockMatrix_M_r, dd - De, NoiseLevel::Noiseless, gamma_v); //Noiseless, LittleNoise
cv::Mat coeffs = perform_projection(blockMatrix_M_r, dd - De); //Noiseless, LittleNoise
cv::Mat coeffs_r;
reorderColumns(coeffs.t(), blockMatrix_M.cols/M, coeffs_r);
cv::Mat coeffs_r_n(coeffs_r.t());
//Undo constraints
cv::Mat sol = cv::Mat::zeros(x0.size(), cv::DataType<std::complex<double> >::type);
for(unsigned int t=0; t < rngs.size(); ++t)
{
cv::Mat sol_i;
cv::gemm(LEC, coeffs_r_n(cv::Range(t*LEC.cols, (t*LEC.cols) + (LEC.cols)), cv::Range::all()), 1.0, cv::Mat(), 1.0, sol_i);
sol_i.copyTo(sol(cv::Range(t*M*K, (t*M*K) + (M*K)), cv::Range::all()));
}
std::cout << "cv::norm(sol): " << cv::norm(sol) << std::endl;
if(cv::norm(sol) < 1e-4 ) {std::cout << "Solution found" << std::endl; break;}
x0 = x0 - sol;
std::cout << "Solution number: " << count << std::endl;
std::cout << "x0: " << x0.t() << std::endl;
}
return cv::Mat(); //mtrc.F();
}