void TriInverse(UpperTriMatrixView<T> U, ptrdiff_t nhi)
{
#ifdef XDEBUG
Matrix<T> U0(U);
#endif
if (U.size() > 0) {
if (nhi == 0) DiagMatrixViewOf(U.diag()).invertSelf();
else if (U.iscm() || U.isrm()) DoInverse(U,nhi);
else {
UpperTriMatrix<T> temp = U;
DoInverse(temp.view(),nhi);
U = temp;
}
}
#ifdef XDEBUG
Matrix<T> eye = U*U0;
if (Norm(eye-T(1)) > 0.0001*(Norm(U0)+Norm(U))) {
cerr<<"UpperTriMatrix Inverse:\n";
cerr<<"U = "<<TMV_Text(U)<<" "<<U0<<endl;
cerr<<"Uinv = "<<U<<endl;
cerr<<"Uinv*U = "<<U*U0<<endl;
cerr<<"U*Uinv = "<<U0*U<<endl;
abort();
}
#endif
}
RT LUDiv<T>::logDet(T* sign) const
{
if (!pimpl->donedet) {
T s;
pimpl->logdet = DiagMatrixViewOf(pimpl->LUx.diag()).logDet(&s);
pimpl->signdet = RT(pimpl->P.det()) * s;
pimpl->donedet = true;
}
if (sign) *sign = pimpl->signdet;
return pimpl->logdet;
}
T LUDiv<T>::det() const
{
if (!pimpl->donedet) {
T s;
pimpl->logdet = DiagMatrixViewOf(pimpl->LUx.diag()).logDet(&s);
pimpl->signdet = RT(pimpl->P.det()) * s;
pimpl->donedet = true;
}
if (pimpl->signdet == T(0)) return T(0);
else return pimpl->signdet * TMV_EXP(pimpl->logdet);
}
void TestTriMatrixArith_C4a()
{
typedef std::complex<T> CT;
tmv::Matrix<T,tmv::RowMajor> a1x(4,4);
for(int i=0;i<4;++i) for(int j=0;j<4;++j) {
a1x(i,j) = T(3+4*i-6*j);
}
a1x(0,0) = 14;
a1x(1,0) = -2;
a1x(2,0) = 7;
a1x(3,0) = -10;
a1x(2,2) = 30;
tmv::Matrix<CT,tmv::RowMajor> ca1x = a1x;
ca1x(2,3) += CT(2,3);
ca1x(1,0) *= CT(0,2);
ca1x.col(1) *= CT(-1,3);
ca1x.row(3) += tmv::Vector<CT>(4,CT(1,9));
tmv::Matrix<T,tmv::ColMajor> a2x = a1x.transpose();
a2x.row(1) *= T(3);
a2x.col(2) -= tmv::Vector<T>(4,4);
tmv::Matrix<CT,tmv::ColMajor> ca2x = ca1x;
ca2x -= a2x;
ca2x *= CT(1,-2);
ca2x(0,0) = CT(0,-5);
tmv::UpperTriMatrixView<T> u1 = a1x.upperTri();
tmv::UpperTriMatrixView<CT> cu1 = ca1x.upperTri();
tmv::UpperTriMatrixView<T> u2 = a2x.upperTri();
tmv::UpperTriMatrixView<CT> cu2 = ca2x.upperTri();
tmv::UpperTriMatrixView<T> u4 = a1x.unitUpperTri();
tmv::UpperTriMatrixView<CT> cu4 = ca1x.unitUpperTri();
tmv::UpperTriMatrixView<T> u5 = a2x.unitUpperTri();
tmv::UpperTriMatrixView<CT> cu5 = ca2x.unitUpperTri();
tmv::DiagMatrixView<T> d1 = DiagMatrixViewOf(a1x.row(0));
tmv::DiagMatrixView<CT> cd1 = DiagMatrixViewOf(ca1x.row(0));
TestMatrixArith4(d1,cd1,u1,cu1,"Diag/UpperTri 1");
TestMatrixArith4(d1,cd1,u2,cu2,"Diag/UpperTri 2");
TestMatrixArith4(d1,cd1,u4,cu4,"Diag/UpperTri 3");
TestMatrixArith4(d1,cd1,u5,cu5,"Diag/UpperTri 4");
#if (XTEST & 1)
tmv::Matrix<T> a3x(12,16);
for(int i=0;i<12;++i) for(int j=0;j<16;++j) a3x(i,j) = T(1-2*i+3*j);
a3x.diag().addToAll(30);
tmv::Matrix<CT> ca3x = a3x*CT(1,-2);
ca3x.diag().addToAll(CT(-22,15));
tmv::DiagMatrixView<T> d3 = DiagMatrixViewOf(a1x.diag());
tmv::DiagMatrixView<CT> cd3 = DiagMatrixViewOf(ca1x.diag());
tmv::UpperTriMatrixView<T> u3 = a3x.subMatrix(0,12,0,16,3,4).upperTri();
tmv::UpperTriMatrixView<CT> cu3 = ca3x.subMatrix(0,12,0,16,3,4).upperTri();
tmv::UpperTriMatrixView<T> u6 = a3x.subMatrix(0,12,0,16,3,4).unitUpperTri();
tmv::UpperTriMatrixView<CT> cu6 = ca3x.subMatrix(0,12,0,16,3,4).unitUpperTri();
TestMatrixArith4(d3,cd3,u1,cu1,"Diag/UpperTri 5");
TestMatrixArith4(d3,cd3,u2,cu2,"Diag/UpperTri 6");
TestMatrixArith4(d3,cd3,u3,cu3,"Diag/UpperTri 7");
TestMatrixArith4(d3,cd3,u4,cu4,"Diag/UpperTri 8");
TestMatrixArith4(d3,cd3,u5,cu5,"Diag/UpperTri 9");
TestMatrixArith4(d3,cd3,u6,cu6,"Diag/UpperTri 10");
TestMatrixArith4(d1,cd1,u3,cu3,"Diag/UpperTri 11");
TestMatrixArith4(d1,cd1,u6,cu6,"Diag/UpperTri 12");
#endif
#if (XTEST & 2)
tmv::LowerTriMatrixView<T> l1 = a1x.lowerTri();
tmv::LowerTriMatrixView<CT> cl1 = ca1x.lowerTri();
tmv::LowerTriMatrixView<T> l2 = a2x.lowerTri();
tmv::LowerTriMatrixView<CT> cl2 = ca2x.lowerTri();
tmv::LowerTriMatrixView<T> l4 = a1x.unitLowerTri();
tmv::LowerTriMatrixView<CT> cl4 = ca1x.unitLowerTri();
tmv::LowerTriMatrixView<T> l5 = a2x.unitLowerTri();
tmv::LowerTriMatrixView<CT> cl5 = ca2x.unitLowerTri();
TestMatrixArith4(d1,cd1,l1,cl1,"Diag/LowerTri 1");
TestMatrixArith4(d1,cd1,l2,cl2,"Diag/LowerTri 2");
TestMatrixArith4(d1,cd1,l4,cl4,"Diag/LowerTri 3");
TestMatrixArith4(d1,cd1,l5,cl5,"Diag/LowerTri 4");
#if (XTEST & 1)
tmv::LowerTriMatrixView<T> l3 = a3x.subMatrix(0,12,0,16,3,4).lowerTri();
tmv::LowerTriMatrixView<CT> cl3 = ca3x.subMatrix(0,12,0,16,3,4).lowerTri();
tmv::LowerTriMatrixView<T> l6 = a3x.subMatrix(0,12,0,16,3,4).unitLowerTri();
tmv::LowerTriMatrixView<CT> cl6 = ca3x.subMatrix(0,12,0,16,3,4).unitLowerTri();
TestMatrixArith4(d3,cd3,l1,cl1,"Diag/LowerTri 5");
TestMatrixArith4(d3,cd3,l2,cl2,"Diag/LowerTri 6");
TestMatrixArith4(d3,cd3,l3,cl3,"Diag/LowerTri 7");
TestMatrixArith4(d3,cd3,l4,cl4,"Diag/LowerTri 8");
TestMatrixArith4(d3,cd3,l5,cl5,"Diag/LowerTri 9");
TestMatrixArith4(d3,cd3,l6,cl6,"Diag/LowerTri 10");
TestMatrixArith4(d1,cd1,l3,cl3,"Diag/LowerTri 11");
TestMatrixArith4(d1,cd1,l6,cl6,"Diag/LowerTri 12");
#endif
#endif
}
template <class T> void TestTriMatrixArith_C6a()
{
typedef std::complex<T> CT;
tmv::Matrix<T,tmv::RowMajor> a1x(4,4);
for(int i=0;i<4;++i) for(int j=0;j<4;++j) {
a1x(i,j) = T(3+4*i-6*j);
}
a1x(0,0) = 14;
a1x(1,0) = -2;
a1x(2,0) = 7;
a1x(3,0) = -10;
a1x(2,2) = 30;
tmv::Matrix<CT,tmv::RowMajor> ca1x = a1x;
ca1x(2,3) += CT(2,3);
ca1x(1,0) *= CT(0,2);
ca1x.col(1) *= CT(-1,3);
ca1x.row(3) += tmv::Vector<CT>(4,CT(1,9));
tmv::Matrix<T,tmv::ColMajor> a2x = a1x.transpose();
a2x.row(1) *= T(3);
a2x.col(2) -= tmv::Vector<T>(4,4);
tmv::Matrix<CT,tmv::ColMajor> ca2x = ca1x;
ca2x -= a2x;
ca2x *= CT(1,-2);
ca2x(0,0) = CT(0,-5);
tmv::UpperTriMatrixView<T> u1 = a1x.upperTri();
tmv::UpperTriMatrixView<CT> cu1 = ca1x.upperTri();
tmv::UpperTriMatrixView<T> u2 = a2x.upperTri();
tmv::UpperTriMatrixView<CT> cu2 = ca2x.upperTri();
tmv::DiagMatrixView<T> d1 = DiagMatrixViewOf(a1x.row(0));
tmv::DiagMatrixView<CT> cd1 = DiagMatrixViewOf(ca1x.row(0));
TestMatrixArith6(u1,cu1,d1,cd1,u1,cu1,"UpperTri/Diag 1");
#if (XTEST & 2)
TestMatrixArith6(u2,cu2,d1,cd1,u1,cu1,"UpperTri/Diag 2");
TestMatrixArith6(u2,cu2,d1,cd1,u2,cu2,"UpperTri/Diag 3");
TestMatrixArith6(u1,cu1,d1,cd1,u2,cu2,"UpperTri/Diag 4");
#endif
#if (XTEST & 1)
tmv::Matrix<T> a3x(12,16);
for(int i=0;i<12;++i) for(int j=0;j<16;++j) a3x(i,j) = T(1-2*i+3*j);
a3x.diag().addToAll(30);
tmv::Matrix<CT> ca3x = a3x*CT(1,-2);
ca3x.diag().addToAll(CT(-22,15));
tmv::DiagMatrixView<T> d3 = DiagMatrixViewOf(a1x.diag());
tmv::DiagMatrixView<CT> cd3 = DiagMatrixViewOf(ca1x.diag());
tmv::UpperTriMatrixView<T> u3 = a3x.subMatrix(0,12,0,16,3,4).upperTri();
tmv::UpperTriMatrixView<CT> cu3 = ca3x.subMatrix(0,12,0,16,3,4).upperTri();
TestMatrixArith6(u3,cu3,d1,cd1,u1,cu1,"UpperTri/Diag 5");
TestMatrixArith6(u1,cu1,d3,cd3,u1,cu1,"UpperTri/Diag 6");
TestMatrixArith6(u2,cu2,d3,cd3,u1,cu1,"UpperTri/Diag 7");
TestMatrixArith6(u1,cu1,d1,cd1,u3,cu3,"UpperTri/Diag 8");
TestMatrixArith6(u2,cu2,d1,cd1,u3,cu3,"UpperTri/Diag 9");
#endif
TestMatrixArith6x(u1,cu1,d1,cd1,"UpperTri/Diag 10");
#if (XTEST & 2)
TestMatrixArith6x(u2,cu2,d1,cd1,"UpperTri/Diag 11");
#endif
#if (XTEST & 1)
TestMatrixArith6x(u3,cu3,d1,cd1,"UpperTri/Diag 12");
TestMatrixArith6x(u1,cu1,d3,cd3,"UpperTri/Diag 13");
TestMatrixArith6x(u2,cu2,d3,cd3,"UpperTri/Diag 14");
#endif
tmv::LowerTriMatrixView<T> l1 = a1x.lowerTri();
tmv::LowerTriMatrixView<CT> cl1 = ca1x.lowerTri();
tmv::LowerTriMatrixView<T> l2 = a2x.lowerTri();
tmv::LowerTriMatrixView<CT> cl2 = ca2x.lowerTri();
TestMatrixArith6(l1,cl1,d1,cd1,l1,cl1,"LowerTri/Diag 1");
#if (XTEST & 2)
TestMatrixArith6(l2,cl2,d1,cd1,l1,cl1,"LowerTri/Diag 2");
TestMatrixArith6(l2,cl2,d1,cd1,l2,cl2,"LowerTri/Diag 3");
TestMatrixArith6(l1,cl1,d1,cd1,l2,cl2,"LowerTri/Diag 4");
#endif
#if (XTEST & 1)
tmv::LowerTriMatrixView<T> l3 = a3x.subMatrix(0,12,0,16,3,4).lowerTri();
tmv::LowerTriMatrixView<CT> cl3 = ca3x.subMatrix(0,12,0,16,3,4).lowerTri();
TestMatrixArith6(l3,cl3,d1,cd1,l1,cl1,"LowerTri/Diag 5");
TestMatrixArith6(l1,cl1,d3,cd3,l1,cl1,"LowerTri/Diag 6");
TestMatrixArith6(l2,cl2,d3,cd3,l1,cl1,"LowerTri/Diag 7");
TestMatrixArith6(l1,cl1,d1,cd1,l3,cl3,"LowerTri/Diag 8");
TestMatrixArith6(l2,cl2,d1,cd1,l3,cl3,"LowerTri/Diag 9");
#endif
TestMatrixArith6x(l1,cl1,d1,cd1,"LowerTri/Diag 10");
#if (XTEST & 2)
TestMatrixArith6x(l2,cl2,d1,cd1,"LowerTri/Diag 11");
#endif
#if (XTEST & 1)
TestMatrixArith6x(l3,cl3,d1,cd1,"LowerTri/Diag 12");
TestMatrixArith6x(l1,cl1,d3,cd3,"LowerTri/Diag 13");
TestMatrixArith6x(l2,cl2,d3,cd3,"LowerTri/Diag 14");
#endif
tmv::UpperTriMatrixView<T> u4 = a1x.unitUpperTri();
tmv::UpperTriMatrixView<CT> cu4 = ca1x.unitUpperTri();
tmv::UpperTriMatrixView<T> u5 = a2x.unitUpperTri();
tmv::UpperTriMatrixView<CT> cu5 = ca2x.unitUpperTri();
TestMatrixArith6(u4,cu4,d1,cd1,u1,cu1,"UpperTri/Diag 15");
TestMatrixArith6(u5,cu5,d1,cd1,u1,cu1,"UpperTri/Diag 16");
TestMatrixArith6(u4,cu4,d1,cd1,u2,cu2,"UpperTri/Diag 17");
TestMatrixArith6(u5,cu5,d1,cd1,u2,cu2,"UpperTri/Diag 18");
#if (XTEST & 1)
tmv::UpperTriMatrixView<T> u6 = a3x.subMatrix(0,12,0,16,3,4).unitUpperTri();
tmv::UpperTriMatrixView<CT> cu6 = ca3x.subMatrix(0,12,0,16,3,4).unitUpperTri();
TestMatrixArith6(u6,cu6,d1,cd1,u1,cu1,"UpperTri/Diag 19");
TestMatrixArith6(u6,cu6,d3,cd3,u1,cu1,"UpperTri/Diag 20");
TestMatrixArith6(u4,cu4,d3,cd3,u1,cu1,"UpperTri/Diag 21");
TestMatrixArith6(u5,cu5,d3,cd3,u1,cu1,"UpperTri/Diag 22");
#endif
TestMatrixArith6x(u4,cu4,d1,cd1,"UpperTri/Diag 23");
#if (XTEST & 2)
TestMatrixArith6x(u5,cu5,d1,cd1,"UpperTri/Diag 24");
#endif
#if (XTEST & 1)
TestMatrixArith6x(u6,cu1,d1,cd1,"UpperTri/Diag 25");
TestMatrixArith6x(u6,cu3,d3,cd3,"UpperTri/Diag 26");
TestMatrixArith6x(u4,cu4,d3,cd3,"UpperTri/Diag 27");
TestMatrixArith6x(u5,cu5,d3,cd3,"UpperTri/Diag 28");
#endif
tmv::LowerTriMatrixView<T> l4 = a1x.unitLowerTri();
tmv::LowerTriMatrixView<CT> cl4 = ca1x.unitLowerTri();
tmv::LowerTriMatrixView<T> l5 = a2x.unitLowerTri();
tmv::LowerTriMatrixView<CT> cl5 = ca2x.unitLowerTri();
TestMatrixArith6(l4,cl4,d1,cd1,l1,cl1,"LowerTri/Diag 15");
TestMatrixArith6(l5,cl5,d1,cd1,l1,cl1,"LowerTri/Diag 16");
TestMatrixArith6(l4,cl4,d1,cd1,l2,cl2,"LowerTri/Diag 17");
TestMatrixArith6(l5,cl5,d1,cd1,l2,cl2,"LowerTri/Diag 18");
#if (XTEST & 1)
tmv::LowerTriMatrixView<T> l6 = a3x.subMatrix(0,12,0,16,3,4).unitLowerTri();
tmv::LowerTriMatrixView<CT> cl6 = ca3x.subMatrix(0,12,0,16,3,4).unitLowerTri();
TestMatrixArith6(l6,cl6,d1,cd1,l1,cl1,"LowerTri/Diag 19");
TestMatrixArith6(l6,cl6,d3,cd3,l1,cl1,"LowerTri/Diag 20");
TestMatrixArith6(l4,cl4,d3,cd3,l1,cl1,"LowerTri/Diag 21");
TestMatrixArith6(l5,cl5,d3,cd3,l1,cl1,"LowerTri/Diag 22");
#endif
TestMatrixArith6x(l4,cl4,d1,cd1,"LowerTri/Diag 23");
#if (XTEST & 2)
TestMatrixArith6x(l5,cl5,d1,cd1,"LowerTri/Diag 24");
#endif
#if (XTEST & 1)
TestMatrixArith6x(l6,cl1,d1,cd1,"LowerTri/Diag 25");
TestMatrixArith6x(l6,cl3,d3,cd3,"LowerTri/Diag 26");
TestMatrixArith6x(l4,cl4,d3,cd3,"LowerTri/Diag 27");
TestMatrixArith6x(l5,cl5,d3,cd3,"LowerTri/Diag 28");
#endif
}
bool Ellipse::doMeasureShapelet(
const std::vector<PixelList>& pix,
const std::vector<BVec>* psf, BVec& b,
int order, int order2, int maxm, DMatrix* bCov) const
{
xdbg<<"Start MeasureShapelet: order = "<<order<<std::endl;
xdbg<<"b.order, sigma = "<<b.getOrder()<<", "<<b.getSigma()<<std::endl;
xdbg<<"el = "<<*this<<std::endl;
if (maxm < 0 || maxm > order) maxm = order;
xdbg<<"order = "<<order<<','<<order2<<','<<maxm<<std::endl;
// ( u )' = exp(-mu)/sqrt(1-gsq) ( 1-g1 -g2 ) ( u-uc )
// ( v ) ( -g2 1+g1 ) ( v-vc )
//
// z' = u' + I v'
// = exp(-mu)/sqrt(1-gsq)
// [ (1-g1)(u-uc)-g2(v-vc)-Ig2(u-uc)+I(1+g1)(v-vc) ]
// = exp(-mu)/sqrt(1-gsq) [ z-zc - g1(z-zc)* -Ig2(z-zc)* ]
// = exp(-mu)/sqrt(1-gsq) ( z-zc - g (z-zc)* )
double sigma = b.getSigma();
double gsq = std::norm(_gamma);
Assert(gsq < 1.);
std::complex<double> mm = exp(-_mu)/sqrt(1.-gsq);
int bsize = (order+1)*(order+2)/2;
xdbg<<"bsize = "<<bsize<<std::endl;
int bsize2 = (order2+1)*(order2+2)/2;
xdbg<<"bsize2 = "<<bsize2<<std::endl;
int ntot = 0;
const int nexp = pix.size();
for(int i=0;i<nexp;++i) ntot += pix[i].size();
dbg<<"ntot = "<<ntot<<" in "<<nexp<<" images\n";
if (ntot < bsize) {
dbg<<"Too few pixels for given order.\n";
return false;
}
DVector I(ntot);
DVector W(ntot);
CDVector Z(ntot);
for(int k=0,n=0;k<nexp;++k) {
double sigma_obs =
psf ?
sqrt(pow(sigma,2)+pow((*psf)[k].getSigma(),2)) :
sigma;
xdbg<<"sigma_obs["<<k<<"] = "<<sigma_obs<<std::endl;
const int npix = pix[k].size();
for(int i=0;i<npix;++i,++n) {
I(n) = pix[k][i].getFlux()*pix[k][i].getInverseSigma();
W(n) = pix[k][i].getInverseSigma();
std::complex<double> z1 = pix[k][i].getPos();
std::complex<double> z2 = mm*((z1-_cen) - _gamma*conj(z1-_cen));
Z(n) = z2 / sigma_obs;
}
}
//xdbg<<"Z = "<<Z<<std::endl;
//xdbg<<"I = "<<I<<std::endl;
//xdbg<<"W = "<<W<<std::endl;
#ifdef USE_TMV
// Figure out a permutation that puts all the m <= maxm first.
// I don't know how to do this with Eigen, but I don't really
// use maxm < order on a regular basis, so only implement this for TMV.
tmv::Permutation P(bsize);
int msize = bsize;
if (maxm < order) {
tmv::Vector<double> mvals(bsize);
for(int n=0,k=0;n<=order;++n) {
for(int m=n;m>=0;m-=2) {
mvals[k++] = m;
if (m > 0) mvals[k++] = m;
}
}
xdbg<<"mvals = "<<mvals<<std::endl;
mvals.sort(P);
xdbg<<"mvals => "<<mvals<<std::endl;
// 00 10 10 20 20 11 30 30 21 21 40 40 31 31 22 50 50 41 41 32 32
// n = 0 1 2 3 4 5 6
// 0size = 1 1 2 2 3 3 4 = (n)/2 + 1
// 1size = 1 3 4 6 7 9 10 = (3*(n-1))/2 + 3
// 2size = 1 3 6 8 11 13 16 = (5*(n-2))/2 + 6
// 3size = 1 3 6 10 13 17 20 = (7*(n-3))/2 + 10
// nsize = (n+1)*(n+2)/2
// msize = (m+1)*(m+2)/2 + (2*m+1)*(n-m)/2
msize = (maxm+1)*(maxm+2)/2 + (2*maxm+1)*(order-maxm)/2;
xdbg<<"msize = "<<msize<<std::endl;
xdbg<<"mvals["<<msize-1<<"] = "<<mvals[msize-1]<<std::endl;
xdbg<<"mvals["<<msize<<"] = "<<mvals[msize]<<std::endl;
Assert(mvals[msize-1] == maxm);
Assert(mvals[msize] == maxm+1);
}
#endif
DMatrix A(ntot,bsize);
Assert(ntot >= bsize); // Should have been addressed by calling routine.
//xdbg<<"A = "<<A<<std::endl;
if (psf) {
for(int k=0,n=0,nx;k<nexp;++k,n=nx) {
xdbg<<"psf = "<<(*psf)[k]<<std::endl;
int psforder = (*psf)[k].getOrder();
int newpsforder = std::max(psforder,order2);
xdbg<<"psforder = "<<psforder<<std::endl;
xdbg<<"newpsforder = "<<newpsforder<<std::endl;
const double psfsigma = (*psf)[k].getSigma();
xdbg<<"sigma = "<<psfsigma<<std::endl;
BVec newpsf(newpsforder,psfsigma);
int psfsize = (*psf)[k].size();
int newpsfsize = newpsf.size();
xdbg<<"psfsize = "<<psfsize<<std::endl;
bool setnew = false;
if (_gamma != 0.) {
DMatrix S(newpsfsize,psfsize);
CalculateGTransform(_gamma,newpsforder,psforder,S);
newpsf.vec() = S * (*psf)[k].vec();
setnew = true;
xdbg<<"newpsf = "<<newpsf<<std::endl;
}
if (real(_mu) != 0.) {
if (setnew) {
DMatrix D(newpsfsize,newpsfsize);
CalculateMuTransform(real(_mu),newpsforder,D);
newpsf.vec() = D * newpsf.vec();
} else {
DMatrix D(newpsfsize,psfsize);
CalculateMuTransform(real(_mu),newpsforder,psforder,D);
newpsf.vec() = D * (*psf)[k].vec();
setnew = true;
}
newpsf.vec() *= exp(2.*real(_mu));
xdbg<<"newpsf => "<<newpsf<<std::endl;
}
if (imag(_mu) != 0.) {
if (setnew) {
#ifdef USE_TMV
DBandMatrix R(newpsfsize,newpsfsize,1,1);
#else
DBandMatrix R(newpsfsize,newpsfsize);
#endif
CalculateThetaTransform(imag(_mu),newpsforder,R);
newpsf.vec() = R * newpsf.vec();
} else {
#ifdef USE_TMV
DBandMatrix R(newpsfsize,psfsize,1,1);
#else
DBandMatrix R(newpsfsize,psfsize);
#endif
CalculateThetaTransform(imag(_mu),newpsforder,psforder,R);
newpsf.vec() = R * (*psf)[k].vec();
setnew = true;
}
xdbg<<"newpsf => "<<newpsf<<std::endl;
}
DMatrix C(bsize2,bsize);
if (setnew) {
CalculatePsfConvolve(newpsf,order2,order,b.getSigma(),C);
} else {
CalculatePsfConvolve((*psf)[k],order2,order,b.getSigma(),C);
}
const int npix = pix[k].size();
nx = n+npix;
DMatrix A1(npix,bsize2);
DVectorView W1 = W.TMV_subVector(n,nx);
MakePsi(A1,Z.TMV_subVector(n,nx),order2,&W1);
TMV_rowRange(A,n,nx) = A1 * C;
}
} else {
DVectorView W1 = TMV_view(W);
MakePsi(A,TMV_vview(Z),order,&W1);
}
const double MAX_CONDITION = 1.e8;
#ifdef USE_TMV
A *= P.transpose();
tmv::MatrixView<double> Am = A.colRange(0,msize);
// I used to use SVD for this, rather than the QRP decomposition.
// But QRP is significantly faster, and the condition estimate
// produced is good enough for what we need here.
// TODO: I need to add a real condition estimator to division methods
// other than SVD in TMV. LAPACK has this functionality...
Am.saveDiv();
Am.divideUsing(tmv::QRP);
Am.qrpd();
xdbg<<"R diag = "<<Am.qrpd().getR().diag()<<std::endl;
if (
#if TMV_VERSION_AT_LEAST(0,65)
Am.qrpd().isSingular() ||
#else
// Fixed a bug in the isSingular function in v0.65.
// Until that is the standard TMV version for DES, use this instead:
(Am.qrpd().getR().diag().minAbs2Element() <
1.e-16 * Am.qrpd().getR().diag().maxAbs2Element()) ||
#endif
DiagMatrixViewOf(Am.qrpd().getR().diag()).condition() >
MAX_CONDITION) {
dbg<<"Poor condition in MeasureShapelet: \n";
dbg<<"R diag = "<<Am.qrpd().getR().diag()<<std::endl;
dbg<<"condition = "<<
DiagMatrixViewOf(Am.qrpd().getR().diag()).condition()<<
std::endl;
dbg<<"det = "<<Am.qrpd().det()<<std::endl;
return false;
}
b.vec().subVector(0,msize) = I/Am;
xdbg<<"b = "<<b<<std::endl;
xdbg<<"Norm(I) = "<<Norm(I)<<std::endl;
xdbg<<"Norm(Am*b) = "<<Norm(Am*b.vec().subVector(0,msize))<<std::endl;
xdbg<<"Norm(I-Am*b) = "<<Norm(I-Am*b.vec().subVector(0,msize))<<std::endl;
b.vec().subVector(msize,bsize).setZero();
b.vec().subVector(0,bsize) = P.transpose() * b.vec().subVector(0,bsize);
xdbg<<"b => "<<b<<std::endl;
if (bCov) {
bCov->setZero();
Am.makeInverseATA(bCov->subMatrix(0,msize,0,msize));
bCov->subMatrix(0,bsize,0,bsize) =
P.transpose() * bCov->subMatrix(0,bsize,0,bsize) * P;
}
#else
Eigen::SVD<DMatrix> svd = A.svd().sort();
const DMatrix& svd_u = svd.matrixU();
const DVector& svd_s = svd.singularValues();
const DMatrix& svd_v = svd.matrixV();
double max = svd_s(0);
double min = svd_s(svd_s.size()-1);
if (max > MAX_CONDITION * min) {
xdbg<<"Poor condition in MeasureShapelet: \n";
xdbg<<"svd = "<<svd_s.transpose()<<std::endl;
xdbg<<"condition = "<<max/min<<std::endl;
return false;
}
// USVtb = I
// b = VS^-1UtI
DVector temp = svd_u.transpose() * I;
temp = svd_s.cwise().inverse().asDiagonal() * temp;
b.vec().TMV_subVector(0,bsize) = svd_v * temp;
if (bCov) {
bCov->setZero();
// (AtA)^-1 = (VSUt USVt)^-1 = (V S^2 Vt)^-1 = V S^-2 Vt
bCov->TMV_subMatrix(0,bsize,0,bsize) =
svd_v *
svd_s.cwise().square().cwise().inverse().asDiagonal() *
svd_v.transpose();
}
#endif
if (!(b(0) > 0.)) {
dbg<<"Calculated b vector has negative b(0):\n";
dbg<<"b = "<<b<<std::endl;
return false;
}
if (order < b.getOrder()) {
xdbg<<"Need to zero the rest of b\n";
xdbg<<"bsize = "<<bsize<<" b.size = "<<b.size()<<std::endl;
// Zero out the rest of the shapelet vector:
b.vec().TMV_subVector(bsize,b.size()).setZero();
}
xdbg<<"Done measure Shapelet\n";
xdbg<<"b = "<<b.vec()<<std::endl;
return true;
}
void LVector::mBasis(
const tmv::ConstVectorView<double>& x, const tmv::ConstVectorView<double>& y,
const tmv::ConstVectorView<double>* invsig,
tmv::MatrixView<T> psi, int order, double sigma)
{
assert (y.size()==x.size());
assert (psi.nrows()==x.size() && psi.ncols()==PQIndex::size(order));
const int N=order;
const int npts_full = x.size();
// It's faster to build the psi matrix in blocks so that more of the matrix stays in
// L1 cache. For a (typical) 256 KB L2 cache size, this corresponds to 8 columns in the
// cache, which is pretty good, since we are usually working on 4 columns at a time,
// plus either X and Y or 3 Lq vectors.
const int BLOCKING_FACTOR=4096;
const int max_npts = std::max(BLOCKING_FACTOR,npts_full);
tmv::DiagMatrix<double> Rsq_full(max_npts);
tmv::Matrix<double> A_full(max_npts,2);
tmv::Matrix<double> tmp_full(max_npts,2);
tmv::DiagMatrix<double> Lmq_full(max_npts);
tmv::DiagMatrix<double> Lmqm1_full(max_npts);
tmv::DiagMatrix<double> Lmqm2_full(max_npts);
for (int ilo=0; ilo<npts_full; ilo+=BLOCKING_FACTOR) {
const int ihi = std::min(npts_full, ilo + BLOCKING_FACTOR);
const int npts = ihi-ilo;
// Cast arguments as diagonal matrices so we can access
// vectorized element-by-element multiplication
tmv::ConstDiagMatrixView<double> X = DiagMatrixViewOf(x.subVector(ilo,ihi));
tmv::ConstDiagMatrixView<double> Y = DiagMatrixViewOf(y.subVector(ilo,ihi));
// Get the appropriate portion of our temporary matrices.
tmv::DiagMatrixView<double> Rsq = Rsq_full.subDiagMatrix(0,npts);
tmv::MatrixView<double> A = A_full.rowRange(0,npts);
tmv::MatrixView<double> tmp = tmp_full.rowRange(0,npts);
// We need rsq values twice, so store them here.
Rsq = X*X;
Rsq += Y*Y;
// This matrix will keep track of real & imag parts
// of prefactor * exp(-r^2/2) (x+iy)^m / sqrt(m!)
// Build the Gaussian factor
for (int i=0; i<npts; i++) A.ref(i,0) = std::exp(-0.5*Rsq(i));
mBasisHelper<T>::applyPrefactor(A.col(0),sigma);
A.col(1).setZero();
// Put 1/sigma factor into every point if doing a design matrix:
if (invsig) A.col(0) *= tmv::DiagMatrixViewOf(invsig->subVector(ilo,ihi));
// Assign the m=0 column first:
psi.col( PQIndex(0,0).rIndex(), ilo,ihi ) = A.col(0);
// Then ascend m's at q=0:
for (int m=1; m<=N; m++) {
int rIndex = PQIndex(m,0).rIndex();
// Multiply by (X+iY)/sqrt(m), including a factor 2 first time through
tmp = Y * A;
A = X * A;
A.col(0) += tmp.col(1);
A.col(1) -= tmp.col(0);
A *= m==1 ? 2. : 1./sqrtn(m);
psi.subMatrix(ilo,ihi,rIndex,rIndex+2) = mBasisHelper<T>::Asign(m%4) * A;
}
// Make three DiagMatrix to hold Lmq's during recurrence calculations
boost::shared_ptr<tmv::DiagMatrixView<double> > Lmq(
new tmv::DiagMatrixView<double>(Lmq_full.subDiagMatrix(0,npts)));
boost::shared_ptr<tmv::DiagMatrixView<double> > Lmqm1(
new tmv::DiagMatrixView<double>(Lmqm1_full.subDiagMatrix(0,npts)));
boost::shared_ptr<tmv::DiagMatrixView<double> > Lmqm2(
new tmv::DiagMatrixView<double>(Lmqm2_full.subDiagMatrix(0,npts)));
for (int m=0; m<=N; m++) {
PQIndex pq(m,0);
int iQ0 = pq.rIndex();
// Go to q=1:
pq.incN();
if (pq.pastOrder(N)) continue;
{ // q == 1
const int p = pq.getP();
const int q = pq.getQ();
const int iQ = pq.rIndex();
Lmqm1->setAllTo(1.); // This is Lm0.
*Lmq = Rsq - (p+q-1.);
*Lmq *= mBasisHelper<T>::Lsign(1.) / (sqrtn(p)*sqrtn(q));
if (m==0) {
psi.col(iQ,ilo,ihi) = (*Lmq) * psi.col(iQ0,ilo,ihi);
} else {
psi.subMatrix(ilo,ihi,iQ,iQ+2) = (*Lmq) * psi.subMatrix(ilo,ihi,iQ0,iQ0+2);
}
}
// do q=2,...
for (pq.incN(); !pq.pastOrder(N); pq.incN()) {
const int p = pq.getP();
const int q = pq.getQ();
const int iQ = pq.rIndex();
// cycle the Lmq vectors
// Lmqm2 <- Lmqm1
// Lmqm1 <- Lmq
// Lmq <- Lmqm2
Lmqm2.swap(Lmqm1);
Lmqm1.swap(Lmq);
double invsqrtpq = 1./sqrtn(p)/sqrtn(q);
*Lmq = Rsq - (p+q-1.);
*Lmq *= mBasisHelper<T>::Lsign(invsqrtpq) * *Lmqm1;
*Lmq -= (sqrtn(p-1)*sqrtn(q-1)*invsqrtpq) * (*Lmqm2);
if (m==0) {
psi.col(iQ,ilo,ihi) = (*Lmq) * psi.col(iQ0,ilo,ihi);
} else {
psi.subMatrix(ilo,ihi,iQ,iQ+2) = (*Lmq) * psi.subMatrix(ilo,ihi,iQ0,iQ0+2);
}
}
}
}
}
static void SymLDL_Decompose(SymBandMatrixView<T> A)
{
TMVAssert(A.uplo() == Lower);
TMVAssert(A.ct() == NonConj);
TMVAssert(A.issym());
TMVAssert(A.nlo() == 1);
const ptrdiff_t N = A.size();
#ifdef XDEBUG
Matrix<T> A0(A);
//cout<<"SymLDLDecompose:\n";
//cout<<"A = "<<TMV_Text(A)<<" "<<A<<endl;
#endif
T* Dj = A.ptr();
T* Lj = A.diag(-1).ptr();
if (A.isdm()) {
T Ax0 = *Lj;
if (*Dj == T(0)) {
#ifdef NOTHROW
std::cerr<<"Non Posdef HermBandMatrix found\n";
exit(1);
#else
throw NonPosDefSymBandLDL<T>(A);
#endif
}
*Lj /= *Dj;
++Dj;
*Dj -= *Lj * Ax0;
++Lj;
} else {
ptrdiff_t step = A.diagstep();
for(ptrdiff_t j=0;j<N-1;++j) {
T Ax0 = *Lj;
if (*Dj == T(0)) {
#ifdef NOTHROW
std::cerr<<"Non Posdef HermBandMatrix found\n";
exit(1);
#else
throw NonPosDefSymBandLDL<T>(A);
#endif
}
*Lj /= *Dj;
Dj += step;
*Dj -= *Lj * Ax0;
Lj += step;
}
}
if (*Dj == T(0)) {
#ifdef NOTHROW
std::cerr<<"Non Posdef HermBandMatrix found\n";
exit(1);
#else
throw NonPosDefSymBandLDL<T>(A);
#endif
}
#ifdef XDEBUG
//cout<<"Done SymLDLDecom\n";
BandMatrix<T> L = A.lowerBand();
L.diag().SetAllTo(T(1));
DiagMatrix<T> D = DiagMatrixViewOf(A.diag());
BandMatrix<T> A2 = L * D * L.transpose();
TMV_RealType(T) normldl = TMV_SQR(Norm(L))*Norm(D);
//cout<<"L = "<<L<<endl;
//cout<<"D = "<<D<<endl;
//cout<<"A2 = "<<A2<<endl;
//cout<<"cf. A0 = "<<A0<<endl;
if (!(Norm(A2-A0) < 0.001*normldl)) {
cerr<<"SymLDLDecomp: A = "<<TMV_Text(A)<<" "<<A0<<endl;
cerr<<"Done: A = "<<A<<endl;
cerr<<"L = "<<L<<endl;
cerr<<"D = "<<D<<endl;
cerr<<"LT = "<<L.transpose()<<endl;
cerr<<"L*D*LT = "<<A2<<endl;
cerr<<"Norm(diff) = "<<Norm(A2-A0);
cerr<<" Norm(L)^2*Norm(D) = "<<normldl<<endl;
abort();
}
#endif
}
static void NonLapLDL_Decompose(SymBandMatrixView<T> A)
{
// For tridiagonal Hermitian band matrices, the sqrt can
// become a significant fraction of the calculation.
// This is unnecessary if we instead decompose A into L D Lt
// where L is unit diagonal.
//
// In this case, the equations become:
//
// A = L0 D L0t
//
// ( A00 Ax0t ) = ( 1 0 ) ( D0 0 ) ( 1 Lx0t )
// ( Ax0 Axx ) = ( Lx0 1 ) ( 0 Dx ) ( 0 1 )
// = ( 1 0 ) ( D0 D0 Lx0t )
// = ( Lx0 1 ) ( 0 Dx )
//
// The equations from this are:
//
// A00 = D0
// Ax0 = Lx0 D0
// Axx = Lx0 D0 Lx0t + Axx'
//
// where Axx' = Dx is the sub-matrix for the next step.
//
TMVAssert(A.uplo() == Lower);
TMVAssert(A.ct() == NonConj);
TMVAssert(A.isherm());
TMVAssert(A.nlo() == 1);
const ptrdiff_t N = A.size();
#ifdef XDEBUG
Matrix<T> A0(A);
//cout<<"LDLDecompose:\n";
//cout<<"A = "<<TMV_Text(A)<<" "<<A<<endl;
#endif
TMV_RealType(T)* Dj = A.realPart().ptr();
T* Lj = A.diag(-1).ptr();
if (A.isdm()) {
for(ptrdiff_t j=0;j<N-1;++j) {
T Ax0 = *Lj;
if (*Dj == TMV_RealType(T)(0)) {
#ifdef NOTHROW
std::cerr<<"Non Posdef HermBandMatrix found\n";
exit(1);
#else
throw NonPosDefHermBandLDL<T>(A);
#endif
}
*Lj /= *Dj;
if (isReal(T())) ++Dj; else Dj+=2;
*Dj -= TMV_REAL(TMV_CONJ(*Lj) * Ax0);
++Lj;
}
} else {
const ptrdiff_t Dstep = A.diag().realPart().step();
const ptrdiff_t Lstep = A.diag().step();
for(ptrdiff_t j=0;j<N-1;++j) {
T Ax0 = *Lj;
if (*Dj == TMV_RealType(T)(0)) {
#ifdef NOTHROW
std::cerr<<"Non Posdef HermBandMatrix found\n";
exit(1);
#else
throw NonPosDefHermBandLDL<T>(A);
#endif
}
*Lj /= *Dj;
Dj += Dstep;
*Dj -= TMV_REAL(TMV_CONJ(*Lj) * Ax0);
Lj += Lstep;
}
}
if (*Dj == TMV_RealType(T)(0)) {
#ifdef NOTHROW
std::cerr<<"Non Posdef HermBandMatrix found\n";
exit(1);
#else
throw NonPosDefHermBandLDL<T>(A);
#endif
}
#ifdef XDEBUG
//cout<<"Done LDLDecompose\n";
BandMatrix<T> L = A.lowerBand();
L.diag().SetAllTo(T(1));
DiagMatrix<T> D = DiagMatrixViewOf(A.diag());
BandMatrix<T> A2 = L * D * L.adjoint();
TMV_RealType(T) normldl = TMV_SQR(Norm(L))*Norm(D);
//cout<<"Done: A = "<<A<<endl;
//cout<<"L = "<<L<<endl;
//cout<<"D = "<<D<<endl;
//cout<<"A2 = "<<A2<<endl;
//cout<<"cf A0 = "<<A0<<endl;
if (!(Norm(A2-A0) < 0.001*normldl)) {
cerr<<"LDL_Decompose: A = "<<TMV_Text(A)<<" "<<A0<<endl;
cerr<<"Done: A = "<<A<<endl;
cerr<<"L = "<<L<<endl;
cerr<<"D = "<<D<<endl;
cerr<<"Lt = "<<L.adjoint()<<endl;
cerr<<"L*D*Lt = "<<A2<<endl;
cerr<<"Norm(diff) = "<<Norm(A2-A0);
cerr<<" Norm(L)^2*Norm(D) = "<<normldl<<endl;
abort();
}
#endif
}
