Fix find shift by correlation

src/c_modules/fine_movement/libimagedeform/include/image_deform.h

@@ -94,6 +94,17 @@ static inline void image_deform_set_shifts(struct ImageDeform *deform, const dou
     memcpy(deform->array, data, deform->grid_h*deform->grid_w*2*sizeof(double));
 }
 
+/**
+ * \brief Get shift at pos (x,y) for axis 1 or 2
+ * \param grid Shift array
+ * \param x x
+ * \param y y
+ * \param axis axis (0 for 'y' axis, 1 for 'x' axis)
+ * \return value
+ */
+double image_deform_get_array(const struct ImageDeform *grid,
+                              int x, int y, int axis);
+
 /**
  * @brief Get shift at pos (x,y)
  *

src/c_modules/fine_movement/libimagedeform/include/image_deform_lc.h

@@ -51,7 +51,18 @@ int  image_deform_lc_init(struct ImageDeformLocalCorrelator *self,
 void image_deform_lc_finalize(struct ImageDeformLocalCorrelator *self);
 
 /**
- * \brief Find global correlator
+ * \brief Find constant correlator
+ */
+void image_deform_lc_find_constant(struct ImageDeformLocalCorrelator *self,
+                                   const struct ImageGrid *img,
+                                   const struct ImageDeform *pre_align,
+                                   const struct ImageGrid *ref_img,
+                                   const struct ImageDeform *ref_pre_align,
+                                   double maximal_shift,
+                                   int subpixels);
+
+/**
+ * \brief Find correlator
  */
 void image_deform_lc_find(struct ImageDeformLocalCorrelator *self,
                           const struct ImageGrid *img,

src/c_modules/fine_movement/libimagedeform/src/image_deform.c

@@ -63,8 +63,8 @@ void image_deform_constant_shift(struct ImageDeform *deform, double dx, double d
  * \param axis axis (0 for 'y' axis, 1 for 'x' axis)
  * \return value
  */
-static double image_deform_get_array(const struct ImageDeform *grid,
-                                     int x, int y, int axis)
+double image_deform_get_array(const struct ImageDeform *grid,
+                              int x, int y, int axis)
 {
     if (x >= grid->grid_w)
         return NAN;
@@ -98,8 +98,8 @@ void image_deform_print(const struct ImageDeform *deform, FILE *out)
 double image_deform_get_shift(const struct ImageDeform *deform,
                               double x, double y, int axis)
 {
-    int xi = x;
-    int yi = y;
+    int xi = floor(x);
+    int yi = floor(y);
 
     double dx = x - xi;
     double dy = y - yi;

src/c_modules/fine_movement/libimagedeform/src/image_deform_lc.c

@@ -16,6 +16,7 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <math.h>
+#include <stdbool.h>
 
 #include <image_deform_lc.h>
 
@@ -57,6 +58,119 @@ static void image_deform_lc_get_area(const struct ImageGrid *img,
     image_grid_get_area(img, pre_aligned_x, pre_aligned_y, area);
 }
 
+void image_deform_lc_find_constant(struct ImageDeformLocalCorrelator *self,
+                                   const struct ImageGrid *img,
+                                   const struct ImageDeform *pre_align,
+                                   const struct ImageGrid *ref_img,
+                                   const struct ImageDeform *ref_pre_align,
+                                   double maximal_shift,
+                                   int subpixels)
+{
+    int i, j;
+    double iter_x, iter_y;
+    double best_x, best_y;
+    double best_corr;
+
+    struct ImageGrid global_area;
+    image_grid_init(&global_area, img->w, img->h);
+    best_x = best_y = 0;
+    image_deform_lc_get_area(img, pre_align, &global_area, img->w/2.0, img->h/2.0);
+    best_corr = image_grid_correlation(&global_area, ref_img);
+    for (iter_y = -maximal_shift; iter_y <= maximal_shift; iter_y += 1.0 / subpixels)
+    for (iter_x = -maximal_shift; iter_x <= maximal_shift; iter_x += 1.0 / subpixels)
+    {
+        image_deform_lc_get_area(img, pre_align, &global_area, iter_x + img->w/2.0, iter_y + img->h/2.0);
+        double corr = image_grid_correlation(&global_area, ref_img);
+        if (corr > best_corr)
+        {
+            best_corr = corr;
+            best_x = iter_x;
+            best_y = iter_y;
+        }
+    }
+    for (i = 0; i < self->grid_h; i++)
+    for (j = 0; j < self->grid_w; j++)
+    {
+        image_deform_set_shift(&self->array, j, i, 0, best_y*self->array.sy);
+        image_deform_set_shift(&self->array, j, i, 1, best_x*self->array.sx);
+    }
+    image_grid_finalize(&global_area);
+}
+
+static void image_deform_lc_find_local(const struct ImageGrid *img,
+                                       const struct ImageDeform *pre_align,
+                                       const struct ImageGrid *ref_img,
+                                       const struct ImageDeform *ref_pre_align,
+                                       int x,
+                                       int y,
+                                       struct ImageGrid *area,
+                                       struct ImageGrid *ref_area,
+                                       double maximal_shift,
+                                       int subpixels,
+                                       double mean_shift_x,
+                                       double mean_shift_y,
+                                       double *shift_x,
+                                       double *shift_y)
+{
+    double iter_x, iter_y;
+    double best_x = 0;
+    double best_y = 0;
+    int num_best = 1;
+    image_deform_lc_get_area(img, pre_align, area, x, y);
+    image_deform_lc_get_area(ref_img, ref_pre_align, ref_area, x, y);
+    double best_corr = image_grid_correlation(area, ref_area);
+    double best_dist = fabs(mean_shift_x) + fabs(mean_shift_y);
+
+    for (iter_y = -maximal_shift; iter_y <= maximal_shift; iter_y += 1.0 / subpixels)
+    for (iter_x = -maximal_shift; iter_x <= maximal_shift; iter_x += 1.0 / subpixels)
+    {
+        if (iter_x == 0 && iter_y == 0)
+        {
+            continue;
+        }
+        image_deform_lc_get_area(img, pre_align, area, x+iter_x, y+iter_y);
+        image_deform_lc_get_area(ref_img, pre_align, ref_area, x, y);
+        double corr1 = image_grid_correlation(area, ref_area);
+        image_deform_lc_get_area(img, pre_align, area, x, y);
+        image_deform_lc_get_area(ref_img, pre_align, ref_area, x-iter_x, y-iter_y);
+        double corr2 = image_grid_correlation(area, ref_area);
+        double corr = (corr1 + corr2)/2;
+        if (corr > best_corr)
+        {
+            best_corr = corr;
+            best_x = iter_x;
+            best_y = iter_y;
+            num_best = 1;
+            best_dist = fabs(iter_x - mean_shift_x) + fabs(iter_y - mean_shift_y);
+        }
+        else if (corr == best_corr)
+        {
+            double dist = fabs(iter_x - mean_shift_x) + fabs(iter_y - mean_shift_y);
+            if (dist < best_dist)
+            {
+                best_x = iter_x;
+                best_y = iter_y;
+                num_best = 1;
+                best_dist = dist;
+            }
+            else if (dist == best_dist)
+            {
+                num_best++;
+            }
+        }
+    }
+    if (num_best == 1)
+    {
+        *shift_x = best_x;
+        *shift_y = best_y;
+    }
+    else
+    {
+        *shift_x = NAN;
+        *shift_y = NAN;
+    }
+}
+
 void image_deform_lc_find(struct ImageDeformLocalCorrelator *self,
                           const struct ImageGrid *img,
                           const struct ImageDeform *pre_align,
@@ -67,11 +181,13 @@ void image_deform_lc_find(struct ImageDeformLocalCorrelator *self,
                           int subpixels)
 {
     int i, j;
+
+    // Find deviations from mean shift
     int w = radius*2+1;
     int h = w;
+    bool hasnan = false;
     struct ImageGrid area;
     struct ImageGrid ref_area;
-
     image_grid_init(&area, w, h);
     image_grid_init(&ref_area, w, h);
 
@@ -82,33 +198,59 @@ void image_deform_lc_find(struct ImageDeformLocalCorrelator *self,
         int x = j * self->pixels;
         int y = i * self->pixels;
 
-        // Init with no shift
-        double best_x = x, best_y = y;
-        image_deform_lc_get_area(img, pre_align, &area, x, y);
-        image_deform_lc_get_area(ref_img, ref_pre_align, &ref_area, x, y);
-        double best_corr = image_grid_correlation(&area, &ref_area);
+        double best_x, best_y;
+        image_deform_lc_find_local(img, pre_align, ref_img, ref_pre_align,
+                                   x, y, &area, &ref_area,
+                                   maximal_shift, subpixels,
+                                   0, 0,
+                                   &best_x, &best_y);
+
+        if (isnan(best_x) || isnan(best_y))
+            hasnan = true;
+        image_deform_set_shift(&self->array, j, i, 0, best_y*self->array.sy);
+        image_deform_set_shift(&self->array, j, i, 1, best_x*self->array.sx);
+    }
 
-        // Find shift where best correlation between area in img and ref_img
-        double iter_x, iter_y;
-        for (iter_y = y - maximal_shift; iter_y <= y + maximal_shift; iter_y += 1.0 / subpixels)
-        for (iter_x = x - maximal_shift; iter_x <= x + maximal_shift; iter_x += 1.0 / subpixels)
+    if (hasnan)
+    {
+        for (i = 0; i < self->grid_h; i++)
+        for (j = 0; j < self->grid_w; j++)
         {
-            if (iter_x == x && iter_y == y)
-            {
-                // We have already calculated it
+            double sy = image_deform_get_shift(&self->array, j, i, 0);
+            double sx = image_deform_get_shift(&self->array, j, i, 1);
+            if (!isnan(sy) && !isnan(sx))
                 continue;
-            }
-            image_deform_lc_get_area(img, pre_align, &area, iter_x, iter_y);
-            double corr = image_grid_correlation(&area, &ref_area);
-            if (corr > best_corr)
+            int ii, jj;
+            double shx = 0, shy = 0;
+            int cnt = 0;
+            for (ii = -1; ii <= 1; ii++)
+            for (jj = -1; jj <= 1; jj++)
             {
-                best_corr = corr;
-                best_x = iter_x;
-                best_y = iter_y;
+                double vy = image_deform_get_array(&self->array, j+jj, i+ii, 0);
+                double vx = image_deform_get_array(&self->array, j+jj, i+ii, 1);
+                if (isnan(vx) || isnan(vy))
+                    continue;
+                shx += vx / self->array.sx;
+                shy += vy / self->array.sy;
+                cnt++;
             }
+
+            if (cnt == 0)
+                continue;
+            shx /= cnt;
+            shy /= cnt;
+
+            int x = j * self->pixels;
+            int y = i * self->pixels;
+            double best_x, best_y;
+            image_deform_lc_find_local(img, pre_align, ref_img, ref_pre_align,
+                                       x, y, &area, &ref_area,
+                                       maximal_shift, subpixels,
+                                       shx, shy,
+                                       &best_x, &best_y);
+            image_deform_set_shift(&self->array, j, i, 0, best_y*self->array.sy);
+            image_deform_set_shift(&self->array, j, i, 1, best_x*self->array.sx);
         }
-        image_deform_set_shift(&self->array, j, i, 0, (best_y - y)*self->array.sy);
-        image_deform_set_shift(&self->array, j, i, 1, (best_x - x)*self->array.sy);
     }
 
     image_grid_finalize(&area);

src/c_modules/fine_movement/libimagedeform/src/image_grid.c

@@ -12,6 +12,7 @@
  * along with this program. If not, see <https://www.gnu.org/licenses/>.
  */
 
+#include <stdio.h>
 #include <stdlib.h>
 #include <math.h>
 
@@ -114,8 +115,8 @@ void image_grid_get_area(const struct ImageGrid *img,
     for (i = 0; i < h; i++)
     for (j = 0; j < w; j++)
     {
-        double px = x + j - (w-1)/2.0;
-        double py = y + i - (h-1)/2.0;
+        double px = x - w/2.0 + j;
+        double py = y - h/2.0 + i;
         double val = image_grid_get_pixel(img, px, py);
         image_grid_set_pixel(area, j, i, val);
     }
@@ -126,7 +127,6 @@ double image_grid_correlation(const struct ImageGrid *image1,
 {
     double top = 0;
     double bottom1 = 0, bottom2 = 0;
-
     if (image1->w != image2->w || image1->h != image2->h)
         return NAN;
 

src/c_modules/fine_movement/module/imagedeform_lc.c

@@ -117,9 +117,81 @@ static PyObject* ImageDeformLC_correlate(PyObject *_self, PyObject *args, PyObje
     return (PyObject *)deform_obj;
 }
 
+static PyObject* ImageDeformLC_correlate_constant(PyObject *_self, PyObject *args, PyObject *kwds)
+{
+    struct ImageDeformLocalCorrelatorObject *self =
+            (struct ImageDeformLocalCorrelatorObject *)_self;
+
+    int subpixels;
+    double maximal_shift;
+    PyObject *img, *ref_img;
+    PyObject *pre_align, *ref_pre_align;
+    static char *kwlist[] = {"img", "pre_align", "ref_img", "ref_pre_align",
+                             "maximal_shift", "subpixels", NULL};
+    if (!PyArg_ParseTupleAndKeywords(args, kwds, "OOOOdi", kwlist,
+                                     &img, &pre_align, &ref_img, &ref_pre_align,
+                                     &maximal_shift, &subpixels))
+    {
+        Py_INCREF(Py_None);
+        return Py_None;
+    }
+
+    if (!PyObject_IsInstance(img, (PyObject *)&ImageGrid))
+    {
+        PyErr_SetString(PyExc_ValueError, "invalid function arguments - need ImageGrid");
+        Py_INCREF(Py_None);
+        return Py_None;
+    }
+    if (!PyObject_IsInstance(ref_img, (PyObject *)&ImageGrid))
+    {
+        PyErr_SetString(PyExc_ValueError, "invalid function arguments - need ImageGrid");
+        Py_INCREF(Py_None);
+        return Py_None;
+    }
+    if ((!Py_IsNone(pre_align)) && !PyObject_IsInstance(pre_align, (PyObject *)&ImageDeform))
+    {
+        PyErr_SetString(PyExc_ValueError, "invalid function arguments - need ImageDeform");
+        Py_INCREF(Py_None);
+        return Py_None;
+    }
+    if ((!Py_IsNone(ref_pre_align)) && !PyObject_IsInstance(ref_pre_align, (PyObject *)&ImageDeform))
+    {
+        PyErr_SetString(PyExc_ValueError, "invalid function arguments - need ImageDeform");
+        Py_INCREF(Py_None);
+        return Py_None;
+    }
+
+    struct ImageGridObject* _img = (struct ImageGridObject*)img;
+    struct ImageGridObject* _ref_img = (struct ImageGridObject*)ref_img;
+
+    struct ImageDeform *pre_align_img = NULL;
+    struct ImageDeform *pre_align_ref_img = NULL;
+    if (!Py_IsNone(pre_align))
+        pre_align_img = &((struct ImageDeformObject *)pre_align)->deform;
+    if (!Py_IsNone(ref_pre_align))
+        pre_align_ref_img = &((struct ImageDeformObject *)ref_pre_align)->deform;
+
+    image_deform_lc_find_constant(&self->correlator, &_img->grid, pre_align_img,
+                                  &_ref_img->grid, pre_align_ref_img,
+                                  maximal_shift, subpixels);
+
+    const struct ImageDeform *deform = &self->correlator.array;
+    PyObject *argList = Py_BuildValue("iiii", deform->image_w, deform->image_h,
+                                              deform->grid_w, deform->grid_h);
+    struct ImageDeformObject *deform_obj =
+        (struct ImageDeformObject *)PyObject_CallObject((PyObject *)&ImageDeform, argList);
+    Py_DECREF(argList);
+
+    image_deform_set_shifts(&deform_obj->deform, deform->array);
+    return (PyObject *)deform_obj;
+}
+
+
 static PyMethodDef ImageDeformLC_methods[] = {
     {"find", (PyCFunction)ImageDeformLC_correlate, METH_VARARGS | METH_KEYWORDS,
      "Find image deformation to best local correlation"},
+    {"find_constant", (PyCFunction)ImageDeformLC_correlate_constant, METH_VARARGS | METH_KEYWORDS,
+     "Find image deformation to best local correlation"},
     {NULL} /* Sentinel */
 };