imageautoguiding.cpp

Go to the documentation of this file.

/*  Image Guide Algorithm
    Copyright (C) 2017 Bob Majewski <[email protected]>

    This application is free software; you can redistribute it and/or
    modify it under the terms of the GNU General Public
    License as published by the Free Software Foundation; either
    version 2 of the License, or (at your option) any later version.
 */

#include "imageautoguiding.h"

#include <qglobal.h>

#include <cmath>

#define SWAP(a, b) \
    tempr = (a);   \
    (a)   = (b);   \
    (b)   = tempr
#define NR_END   1
#define FREE_ARG char *

#define TWOPI   6.28318530717959
#define FFITMAX 0.05

//void ImageAutoGuiding1(float *ref,float *im,int n,float *xshift,float *yshift);

float ***f3tensorSP(long nrl, long nrh, long ncl, long nch, long ndl, long ndh);
void free_f3tensorSP(float ***t, long nrl, long nrh, long ncl, long nch, long ndl, long ndh);

float **matrixSP(long nrl, long nrh, long ncl, long nch);

void free_matrixSP(float **m, long nrl, long nrh, long ncl, long nch);
void nrerrorNR(void);

void fournNR(float data[], unsigned long nn[], long ndim, long isign);

void rlft3NR(float ***data, float **speq, unsigned long nn1, unsigned long nn2, unsigned long nn3, long isign);

void ShiftEST(float ***testimage, float ***refimage, int n, float *xshift, float *yshift, int k);

namespace ImageAutoGuiding
{
void ImageAutoGuiding1(float *ref, float *im, int n, float *xshift, float *yshift)
{
    float ***RefImage, ***TestImage;
    int i, j, k;
    float x, y;

    /* Allocate memory */

    RefImage  = f3tensorSP(1, 1, 1, n, 1, n);
    TestImage = f3tensorSP(1, 1, 1, n, 1, n);

    /* Load Data */

    k = 0;

    for (j = 1; j <= n; ++j)
    {
        for (i = 1; i <= n; ++i)
        {
            RefImage[1][j][i]  = ref[k];
            TestImage[1][j][i] = im[k];

            k += 1;
        }
    }

    /* Calculate Image Shifts  */

    ShiftEST(TestImage, RefImage, n, &x, &y, 1);

    *xshift = x;
    *yshift = y;

    /* free memory */

    free_f3tensorSP(RefImage, 1, 1, 1, n, 1, n);
    free_f3tensorSP(TestImage, 1, 1, 1, n, 1, n);
}
}

// Calculates Image Shifts

void ShiftEST(float ***testimage, float ***refimage, int n, float *xshift, float *yshift, int k)
{
    int ix, iy, nh, nhplusone;
    double deltax, deltay, fx2sum, fy2sum, phifxsum, phifysum, fxfysum;
    double fx, fy, ff, fn, re, im, testre, testim, rev, imv, phi;
    double power, dem, f2, f2limit;
    float **speq;

    f2limit = FFITMAX * FFITMAX;

    speq = matrixSP(1, 1, 1, 2 * n);

    nh        = n / 2;
    nhplusone = nh + 1;

    fn = ((float)n);
    ff = 1.0 / fn;

    /* FFT of Reference */

    for (ix = 1; ix <= 2 * n; ++ix)
    {
        speq[1][ix] = 0.0;
    }

    rlft3NR(refimage, speq, 1, n, n, 1);

    /* FFT of Test Image */

    for (ix = 1; ix <= 2 * n; ++ix)
    {
        speq[1][ix] = 0.0;
    }

    rlft3NR(testimage, speq, 1, n, n, 1);

    /* Solving for slopes  */

    fx2sum = 0.0;
    fy2sum = 0.0;

    phifxsum = 0.0;
    phifysum = 0.0;

    fxfysum = 0.0;

    for (ix = 1; ix <= n; ++ix)
    {
        if (ix <= nhplusone)
        {
            fx = ff * ((float)(ix - 1));
        }
        else
        {
            fx = -ff * ((float)(n + 1 - ix));
        }

        for (iy = 1; iy <= nh; ++iy)
        {
            fy = ff * ((float)(iy - 1));

            f2 = fx * fx + fy * fy;

            /* Limit to Low Spatial Frequencies */

            if (f2 < f2limit)
            {
                /* Real part reference image */

                re = refimage[k][ix][2 * iy - 1];

                /* Imaginary part reference image */

                im = refimage[k][ix][2 * iy];

                power = re * re + im * im;

                /* Real part test image */

                testre = testimage[k][ix][2 * iy - 1];

                /* Imaginary part image */

                testim = testimage[k][ix][2 * iy];

                rev = re * testre + im * testim;
                imv = re * testim - im * testre;

                /* Find Phase */

                phi = atan2(imv, rev);

                fx2sum += power * fx * fx;
                fy2sum += power * fy * fy;

                phifxsum += power * fx * phi;
                phifysum += power * fy * phi;

                fxfysum += power * fx * fy;
            }
        }
    }

    /* calculate subpixel shift */

    dem = fx2sum * fy2sum - fxfysum * fxfysum;

    deltax = (phifxsum * fy2sum - fxfysum * phifysum) / (dem * TWOPI);
    deltay = (phifysum * fx2sum - fxfysum * phifxsum) / (dem * TWOPI);

    free_matrixSP(speq, 1, 1, 1, 2 * n);

    /* You can change the shift mapping here */

    *xshift = deltax;
    *yshift = deltay;
}

// 2 and 3 Dimensional FFT Routine for Real Data
// Numerical Recipes in C Second Edition
// The Art of Scientific Computing
// 1999

void rlft3NR(float ***data, float **speq, unsigned long nn1, unsigned long nn2, unsigned long nn3, long isign)
{
    void fournNR(float data[], unsigned long nn[], long ndim, long isign);
    void nrerror();
    unsigned long i1, i2, i3, j1, j2, j3, nn[4], ii3;
    double theta, wpi, wpr, wtemp;
    float c1, c2, h1r, h1i, h2r, h2i;
    float wi, wr;

    if ((unsigned long)(1 + &data[nn1][nn2][nn3] - &data[1][1][1]) != nn1 * nn2 * nn3)
        nrerrorNR();
    c1    = 0.5;
    c2    = -0.5 * isign;
    theta = isign * (6.28318530717959 / nn3);
    wtemp = sin(0.5 * theta);
    wpr   = -2.0 * wtemp * wtemp;
    wpi   = sin(theta);
    nn[1] = nn1;
    nn[2] = nn2;
    nn[3] = nn3 >> 1;
    if (isign == 1)
    {
        fournNR(&data[1][1][1] - 1, nn, 3, isign);
        for (i1 = 1; i1 <= nn1; i1++)
            for (i2 = 1, j2 = 0; i2 <= nn2; i2++)
            {
                speq[i1][++j2] = data[i1][i2][1];
                speq[i1][++j2] = data[i1][i2][2];
            }
    }
    for (i1 = 1; i1 <= nn1; i1++)
    {
        j1 = (i1 != 1 ? nn1 - i1 + 2 : 1);
        wr = 1.0;
        wi = 0.0;
        for (ii3 = 1, i3 = 1; i3 <= (nn3 >> 2) + 1; i3++, ii3 += 2)
        {
            for (i2 = 1; i2 <= nn2; i2++)
            {
                if (i3 == 1)
                {
                    j2               = (i2 != 1 ? ((nn2 - i2) << 1) + 3 : 1);
                    h1r              = c1 * (data[i1][i2][1] + speq[j1][j2]);
                    h1i              = c1 * (data[i1][i2][2] - speq[j1][j2 + 1]);
                    h2i              = c2 * (data[i1][i2][1] - speq[j1][j2]);
                    h2r              = -c2 * (data[i1][i2][2] + speq[j1][j2 + 1]);
                    data[i1][i2][1]  = h1r + h2r;
                    data[i1][i2][2]  = h1i + h2i;
                    speq[j1][j2]     = h1r - h2r;
                    speq[j1][j2 + 1] = h2i - h1i;
                }
                else
                {
                    j2                    = (i2 != 1 ? nn2 - i2 + 2 : 1);
                    j3                    = nn3 + 3 - (i3 << 1);
                    h1r                   = c1 * (data[i1][i2][ii3] + data[j1][j2][j3]);
                    h1i                   = c1 * (data[i1][i2][ii3 + 1] - data[j1][j2][j3 + 1]);
                    h2i                   = c2 * (data[i1][i2][ii3] - data[j1][j2][j3]);
                    h2r                   = -c2 * (data[i1][i2][ii3 + 1] + data[j1][j2][j3 + 1]);
                    data[i1][i2][ii3]     = h1r + wr * h2r - wi * h2i;
                    data[i1][i2][ii3 + 1] = h1i + wr * h2i + wi * h2r;
                    data[j1][j2][j3]      = h1r - wr * h2r + wi * h2i;
                    data[j1][j2][j3 + 1]  = -h1i + wr * h2i + wi * h2r;
                }
            }
            wr = (wtemp = wr) * wpr - wi * wpi + wr;
            wi = wi * wpr + wtemp * wpi + wi;
        }
    }
    if (isign == -1)
        fournNR(&data[1][1][1] - 1, nn, 3, isign);
}

// Multi-Dimensional FFT Routine for Complex Data
// Numerical Recipes in C Second Edition
// The Art of Scientific Computing
// 1999
// Used in rlft3

void fournNR(float data[], unsigned long nn[], long ndim, long isign)
{
    long idim;
    unsigned long i1, i2rev, i3rev, ip1, ip2, ip3, ifp1, ifp2;
    unsigned long i2, i3;
    unsigned long ibit, k1 = 0, k2, n, nprev, nrem, ntot;
    float wi, wr, tempi, tempr;
    double theta, wpi, wpr, wtemp;

    for (ntot = 1, idim = 1; idim <= ndim; idim++)
        ntot *= nn[idim];
    nprev = 1;
    for (idim = ndim; idim >= 1; idim--)
    {
        n     = nn[idim];
        nrem  = ntot / (n * nprev);
        ip1   = nprev << 1;
        ip2   = ip1 * n;
        ip3   = ip2 * nrem;
        i2rev = 1;
        for (i2 = 1; i2 <= ip2; i2 += ip1)
        {
            if (i2 < i2rev)
            {
                for (i1 = i2; i1 <= i2 + ip1 - 2; i1 += 2)
                {
                    for (i3 = i1; i3 <= ip3; i3 += ip2)
                    {
                        i3rev = i2rev + i3 - i2;
                        SWAP(data[i3], data[i3rev]);
                        SWAP(data[i3 + 1], data[i3rev + 1]);
                    }
                }
            }
            ibit = ip2 >> 1;
            while (ibit >= ip1 && i2rev > ibit)
            {
                i2rev -= ibit;
                ibit >>= 1;
            }
            i2rev += ibit;
        }
        ifp1 = ip1;
        while (ifp1 < ip2)
        {
            ifp2  = ifp1 << 1;
            theta = isign * 6.28318530717959 / (ifp2 / ip1);
            wtemp = sin(0.5 * theta);
            wpr   = -2.0 * wtemp * wtemp;
            wpi   = sin(theta);
            wr    = 1.0;
            wi    = 0.0;
            for (i3 = 1; i3 <= ifp1; i3 += ip1)
            {
                for (i1 = i3; i1 <= i3 + ip1 - 2; i1 += 2)
                {
                    for (i2 = i1; i2 <= ip3; i2 += ifp2)
                    {
                        k1           = i2;
                        k2           = k1 + ifp1;
                        tempr        = (double)wr * data[k2] - (double)wi * data[k2 + 1];
                        tempi        = (double)wr * data[k2 + 1] + (double)wi * data[k2];
                        data[k2]     = data[k1] - tempr;
                        data[k2 + 1] = data[k1 + 1] - tempi;
                        data[k1] += tempr;
                        data[k1 + 1] += tempi;
                    }
                }
                wr = (wtemp = wr) * wpr - wi * wpi + wr;
                wi = wi * wpr + wtemp * wpi + wi;
            }
            ifp1 = ifp2;
        }
        nprev *= n;
    }
}

void nrerrorNR(void)
{
}

// Numerical Recipes memory allocation routines
// One based arrays

float **matrixSP(long nrl, long nrh, long ncl, long nch)
/* allocate a float matrix with subscript range m[nrl..nrh][ncl..nch] */
{
    long i, nrow = nrh - nrl + 1, ncol = nch - ncl + 1;
    float **m;

    /* allocate pointers to rows */
    m = (float **)malloc((size_t)((nrow + NR_END) * sizeof(float *)));
    if (!m)
        nrerrorNR();
    m += NR_END;
    m -= nrl;

    /* allocate rows and set pointers to them */
    m[nrl] = (float *)malloc((size_t)((nrow * ncol + NR_END) * sizeof(float)));
    if (!m[nrl])
        nrerrorNR();
    m[nrl] += NR_END;
    m[nrl] -= ncl;

    for (i = nrl + 1; i <= nrh; i++)
        m[i] = m[i - 1] + ncol;

    /* return pointer to array of pointers to rows */
    return m;
}

float ***f3tensorSP(long nrl, long nrh, long ncl, long nch, long ndl, long ndh)
/* allocate a float 3tensor with range t[nrl..nrh][ncl..nch][ndl..ndh] */
{
    long i, j, nrow = nrh - nrl + 1, ncol = nch - ncl + 1, ndep = ndh - ndl + 1;
    float ***t;

    /* allocate pointers to pointers to rows */
    t = (float ***)malloc((size_t)((nrow + NR_END) * sizeof(float **)));
    if (!t)
        nrerrorNR();
    t += NR_END;
    t -= nrl;

    /* allocate pointers to rows and set pointers to them */
    t[nrl] = (float **)malloc((size_t)((nrow * ncol + NR_END) * sizeof(float *)));
    if (!t[nrl])
        nrerrorNR();
    t[nrl] += NR_END;
    t[nrl] -= ncl;

    /* allocate rows and set pointers to them */
    t[nrl][ncl] = (float *)malloc((size_t)((nrow * ncol * ndep + NR_END) * sizeof(float)));
    if (!t[nrl][ncl])
        nrerrorNR();
    t[nrl][ncl] += NR_END;
    t[nrl][ncl] -= ndl;

    for (j = ncl + 1; j <= nch; j++)
        t[nrl][j] = t[nrl][j - 1] + ndep;
    for (i = nrl + 1; i <= nrh; i++)
    {
        t[i]      = t[i - 1] + ncol;
        t[i][ncl] = t[i - 1][ncl] + ncol * ndep;
        for (j = ncl + 1; j <= nch; j++)
            t[i][j] = t[i][j - 1] + ndep;
    }

    /* return pointer to array of pointers to rows */
    return t;
}

void free_matrixSP(float **m, long nrl, long nrh, long ncl, long nch)
/* free a float matrix allocated by matrix() */
{
    Q_UNUSED(nrh);
    Q_UNUSED(nch);
    free((FREE_ARG)(m[nrl] + ncl - NR_END));
    free((FREE_ARG)(m + nrl - NR_END));
}

void free_f3tensorSP(float ***t, long nrl, long nrh, long ncl, long nch, long ndl, long ndh)
/* free a float f3tensor allocated by f3tensor() */
{
    Q_UNUSED(nrh);
    Q_UNUSED(nch);
    Q_UNUSED(ndh);
    free((FREE_ARG)(t[nrl][ncl] + ndl - NR_END));
    free((FREE_ARG)(t[nrl] + ncl - NR_END));
    free((FREE_ARG)(t + nrl - NR_END));
}