void CPUTracker::FFT2D::Apply(float* d)
{
int w = xfft.nfft();
int h = yfft.nfft();
std::complex<float>* tmpsrc = ALLOCA_ARRAY(std::complex<float>, h);
std::complex<float>* tmpdst = ALLOCA_ARRAY(std::complex<float>, h);
for(int y=0;y<h;y++) {
for (int x=0;x<w;x++)
tmpsrc[x]=d[y*w+x];
xfft.transform(tmpsrc, &cbuf[y*w]);
}
for (int x=0;x<w;x++) {
for (int y=0;y<h;y++)
tmpsrc[y]=cbuf[y*w+x];
yfft.transform(tmpsrc, tmpdst);
for (int y=0;y<h;y++)
cbuf[y*w+x]=tmpdst[y];
}
// copy and shift
for (int y=0;y<h;y++) {
for (int x=0;x<w;x++) {
int dx=(x+w/2)%w;
int dy=(y+h/2)%h;
auto v=cbuf[x+y*w];
d[dx+dy*w] = v.real()*v.real() + v.imag()*v.imag();
}
}
// delete[] tmpsrc;
// delete[] tmpdst;
}
// Profile is complex_t[nr*2]
scalar_t CPUTracker::QI_ComputeOffset(complex_t* profile, int nr, int axisForDebug)
{
complex_t* reverse = ALLOCA_ARRAY(complex_t, nr*2);
complex_t* fft_out = ALLOCA_ARRAY(complex_t, nr*2);
complex_t* fft_out2 = ALLOCA_ARRAY(complex_t, nr*2);
for(int x=0;x<nr*2;x++)
reverse[x] = profile[nr*2-1-x];
qi_fft_forward->transform(profile, fft_out);
qi_fft_forward->transform(reverse, fft_out2); // fft_out2 contains fourier-domain version of reverse profile
// multiply with conjugate
for(int x=0;x<nr*2;x++)
fft_out[x] *= conjugate(fft_out2[x]);
qi_fft_backward->transform(fft_out, fft_out2);
#ifdef QI_DEBUG
cmp_cpu_qi_fft_out.assign(fft_out2, fft_out2+nr*2);
#endif
// fft_out2 now contains the autoconvolution
// convert it to float
scalar_t* autoconv = ALLOCA_ARRAY(scalar_t, nr*2);
for(int x=0;x<nr*2;x++) {
autoconv[x] = fft_out2[(x+nr)%(nr*2)].real();
}
scalar_t maxPos = ComputeMaxInterp<scalar_t, QI_LSQFIT_NWEIGHTS>::Compute(autoconv, nr*2, QIWeights);
return (maxPos - nr) * (3.14159265359f / 4);
}
// Profile is complex_t[nr*2]
scalar_t CPUTracker::QuadrantAlign_ComputeOffset(complex_t* profile, complex_t* zlut_prof_fft, int nr, int axisForDebug)
{
complex_t* fft_out = ALLOCA_ARRAY(complex_t, nr*2);
// WriteComplexImageAsCSV("qa_profile.txt", profile, nr*2, 1);
qa_fft_forward->transform(profile, fft_out);
// multiply with conjugate
for(int x=0;x<nr*2;x++)
fft_out[x] *= conjugate(zlut_prof_fft[x]);
qa_fft_backward->transform(fft_out, profile);
#ifdef QI_DEBUG
cmp_cpu_qi_fft_out.assign(fft_out2, fft_out2+nr*2);
#endif
// profile now contains the autoconvolution
// convert it to float
scalar_t* autoconv = ALLOCA_ARRAY(scalar_t, nr*2);
for(int x=0;x<nr*2;x++) {
autoconv[x] = profile[(x+nr)%(nr*2)].real();
}
// WriteImageAsCSV("qa_autoconv.txt", autoconv, nr*2, 1);
scalar_t maxPos = ComputeMaxInterp<scalar_t, QI_LSQFIT_NWEIGHTS>::Compute(autoconv, nr*2, QIWeights);
return (maxPos - nr) * (3.14159265359f / 4);
}
/*
- Compute quadrants
- Build interpolated profile from ZLUT
- Align X & Y against profile with FFT
Difference with QI: No mirroring of profile, instead of mirror we use ZLUT profile
*/
vector3f CPUTracker::QuadrantAlign(vector3f pos, int beadIndex, int angularStepsPerQuadrant, bool& boundaryHit)
{
float* zlut = GetRadialZLUT(beadIndex);
int res=zlut_res;
complex_t* profile = ALLOCA_ARRAY(complex_t, res*2);
complex_t* concat0 = ALLOCA_ARRAY(complex_t, res*2);
complex_t* concat1 = concat0 + res;
memset(concat0, 0, sizeof(complex_t)*res*2);
int zp0 = clamp( (int)pos.z, 0, zlut_planes - 1);
int zp1 = clamp( (int)pos.z + 1, 0, zlut_planes - 1);
float* zlut0 = &zlut[ res * zp0 ];
float* zlut1 = &zlut[ res * zp1 ];
float frac = pos.z - (int)pos.z;
for (int r=0;r<zlut_res;r++) {
// Interpolate plane
double zlutValue = Lerp(zlut0[r], zlut1[r], frac);
concat0[res-r-1] = concat1[r] = zlutValue;
}
// WriteComplexImageAsCSV("qa_zlutprof.txt", concat0, res,2);
qa_fft_forward->transform(concat0, profile);
scalar_t* buf = ALLOCA_ARRAY(scalar_t, res*4);
scalar_t* q0=buf, *q1=buf+res, *q2=buf+res*2, *q3=buf+res*3;
boundaryHit = CheckBoundaries(vector2f(pos.x,pos.y), zlut_maxradius);
for (int q=0;q<4;q++) {
scalar_t *quadrantProfile = buf+q*res;
ComputeQuadrantProfile(quadrantProfile, res, angularStepsPerQuadrant, q, zlut_minradius, zlut_maxradius, vector2f(pos.x,pos.y));
NormalizeRadialProfile(quadrantProfile, res);
}
//WriteImageAsCSV("qa_qdr.txt" , buf, res,4);
float pixelsPerProfLen = (zlut_maxradius-zlut_minradius)/zlut_res;
boundaryHit = false;
// Build Ix = [ qL(-r) qR(r) ]
// qL = q1 + q2 (concat0)
// qR = q0 + q3 (concat1)
for(int r=0;r<res;r++) {
concat0[res-r-1] = q1[r]+q2[r];
concat1[r] = q0[r]+q3[r];
}
scalar_t offsetX = QuadrantAlign_ComputeOffset(concat0, profile, res, 0);
// Build Iy = [ qB(-r) qT(r) ]
// qT = q0 + q1
// qB = q2 + q3
for(int r=0;r<res;r++) {
concat1[r] = q0[r]+q1[r];
concat0[res-r-1] = q2[r]+q3[r];
}
scalar_t offsetY = QuadrantAlign_ComputeOffset(concat0, profile, res, 1);
return vector3f(pos.x + offsetX * pixelsPerProfLen, pos.y + offsetY * pixelsPerProfLen, pos.z);
}
void ViewWindow::Select(float sx, float sy, int w, int h, bool box) {
int bufsize = 10000; // FIXME: Find some way to calculate this value
uint *buffer;
int vp[4]; // viewport
make_current();
buffer = ALLOCA_ARRAY(uint, bufsize);
selector_list = ALLOCA_ARRAY(ViewSelector*, bufsize);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
sy = this->h() - sy;
glGetIntegerv(GL_VIEWPORT, vp);
gluPickMatrix(sx + w / 2, sy - h / 2, w, h, vp);
SetupProjectionMatrix();
glSelectBuffer(bufsize, (uint *)buffer);
glRenderMode(GL_SELECT);
glInitNames();
bSelecting = true;
selector_count = 1;
glPushName(0);
DrawScene();
glPopName();
bSelecting = false;
int a, hits;
if ((hits = glRenderMode(GL_RENDER)) != 0) {
Vector3 pos;
if (!box) {
pos = FindSelCoords(sx + w / 2, sy + h / 2);
logger.Trace(NL_Msg, "SelPos: %d, %d, %d\n", (int) pos.x, (int) pos.y, (int) pos.z);
}
uint *pBuf = buffer;
ViewSelector *best = 0;
float bestscore = 0.;
bool bRedraw = false;
for (a = 0; a < hits; a++) {
uint num = pBuf [0];
pBuf += 3;
for (uint b=0;b<num;b++)
{
int sl_index = *(pBuf ++) - 1;
// only the top of the name stack
if (b < num - 1)
continue;
if (sl_index >= 0 && sl_index < bufsize)
{
ViewSelector *sel = selector_list [sl_index];
assert (sel);
if (box)
{
sel->Toggle (pos, !sel->IsSelected ());
bRedraw=true;
}
else
{
float score = sel->Score(pos, GetCamDistance (pos));
if (!best || score < bestscore) {
best=sel;
bestscore=score;
}
}
}
}
}
if (best)
{
bool bSelect = !best->IsSelected ();
best->Toggle (pos, bSelect);
bRedraw=true;
}
if (bRedraw)
editor->SelectionUpdated ();
}
}
