Xmipp  v3.23.11-Nereus
angular_continuous_assign.cpp
Go to the documentation of this file.
1 /***************************************************************************
2  *
3  * Authors: Slavica Jonic slavica.jonic@a3.epfl.ch (2004)
4  * Carlos Oscar Sanchez Sorzano coss.eps@ceu.es
5  *
6  * Biomedical Imaging Group, EPFL.
7  *
8  * This program is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2 of the License, or
11  * (at your option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with this program; if not, write to the Free Software
20  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA
21  * 02111-1307 USA
22  *
23  * All comments concerning this program package may be sent to the
24  * e-mail address 'xmipp@cnb.csic.es'
25  ***************************************************************************/
26 
27 #include "angular_continuous_assign.h"
28 
29 #include <core/bilib/configs.h>
30 #include <core/bilib/linearalgebra.h>
31 #include <core/bilib/error.h>
32 #include <core/bilib/messagedisplay.h>
33 #include <core/bilib/getputd.h>
34 #include <core/bilib/kernel.h>
35 #include <core/bilib/kerneldiff1.h>
36 #include <core/bilib/tsplinebasis.h>
37 #include <core/bilib/tboundaryconvention.h>
38 #include <core/bilib/changebasis.h>
39 #include <core/bilib/dft.h>
40 
41 #include <core/xmipp_funcs.h>
42 #include <core/args.h>
43 #include <core/xmipp_fft.h>
44 #include <data/mask.h>
45 
46 /* ------------------------------------------------------------------------- */
47 // Prototypes
48 int cstregistration(struct cstregistrationStruct *Data);
49 
50 // Empty constructor =======================================================
51 ProgAngularContinuousAssign::ProgAngularContinuousAssign()
52 {
53  produces_a_metadata = true;
54 }
55 
56 // Read arguments ==========================================================
57 void ProgAngularContinuousAssign::readParams()
58 {
59  XmippMetadataProgram::readParams();
60  fn_ref = getParam("--ref");
61  gaussian_DFT_sigma = getDoubleParam("--gaussian_Fourier");
62  gaussian_Real_sigma = getDoubleParam("--gaussian_Real");
63  weight_zero_freq = getDoubleParam("--zerofreq_weight");
64  max_no_iter = getIntParam("--max_iter");
65  max_shift = getDoubleParam("--max_shift");
66  max_angular_change = getDoubleParam("--max_angular_change");
67 }
68 
69 // Show ====================================================================
70 void ProgAngularContinuousAssign::show()
71 {
72  if (!verbose)
73  return;
74  XmippMetadataProgram::show();
75  std::cout << "Reference volume: " << fn_ref << std::endl
76  << "Gaussian Fourier: " << gaussian_DFT_sigma << std::endl
77  << "Gaussian Real: " << gaussian_Real_sigma << std::endl
78  << "Zero-frequency weight:"<< weight_zero_freq << std::endl
79  << "Max. Iter: " << max_no_iter << std::endl
80  << "Max. Shift: " << max_shift << std::endl
81  << "Max. Angular Change: " << max_angular_change << std::endl
82  ;
83 }
84 
85 // usage ===================================================================
86 void ProgAngularContinuousAssign::defineParams()
87 {
88  addUsageLine("Make a continuous angular assignment");
89  addUsageLine("+This program assigns Euler angles to experimental projections by ");
90  addUsageLine("+minimizing the difference between the given experimental image and ");
91  addUsageLine("+the central slice of a reference volume in Fourier space. All ");
92  addUsageLine("+interpolations are based on a B-spline model of the images and the ");
93  addUsageLine("+volume. The translations are also optimized. Since an iterative ");
94  addUsageLine("+optimization is performed, it must be initialized with some rough ");
95  addUsageLine("+estimate of the angles. The output of xmipp_angular_predict can be ");
96  addUsageLine("+used as initialization without any transformation.");
97  addUsageLine("+The method is fully described at http://www.ncbi.nlm.nih.gov/pubmed/15885434");
98  defaultComments["-i"].clear();
99  defaultComments["-i"].addComment("Metadata with initial alignment");
100  defaultComments["-o"].clear();
101  defaultComments["-o"].addComment("Metadata with output alignment");
102  XmippMetadataProgram::defineParams();
103  addParamsLine(" --ref <volume> : Reference volume");
104  addParamsLine(" [--gaussian_Fourier <s=0.5>] : Weighting sigma in Fourier space");
105  addParamsLine(" :+Small values of this parameter concentrate ");
106  addParamsLine(" :+the optimization on low frequencies. This ");
107  addParamsLine(" :+value should be below 1.");
108  addParamsLine(" [--gaussian_Real <s=0.5>] : Weighting sigma in Real space");
109  addParamsLine(" :+Small values of this parameter concentrate ");
110  addParamsLine(" :+the optimization in the image center. This ");
111  addParamsLine(" :+value should be below 1.");
112  addParamsLine(" [--zerofreq_weight <s=0. >] : Zero-frequency weight");
113  addParamsLine(" [--max_iter <max=60>] : Maximum number of iterations");
114  addParamsLine(" :+Convergence might be reached before this number of iterations");
115  addParamsLine(" [--max_shift <s=-1>] : Maximum shift allowed");
116  addParamsLine(" :+Use this option to limit the maximum shift that the ");
117  addParamsLine(" :+optimization process can find. If the minimum is ");
118  addParamsLine(" :+found beyond this limit (note it is an absolute limit ");
119  addParamsLine(" :+on the shift and it is not relative to the initial shift), ");
120  addParamsLine(" :+then the solution given by the initial guess is kept ");
121  addParamsLine(" :+since the optimized one looks suspicious.");
122  addParamsLine(" [--max_angular_change <a=-1>]: Maximum angular change allowed");
123  addParamsLine(" :+Use this option to limit the maximum angular ");
124  addParamsLine(" :+change with respect to the initial solution ");
125  addParamsLine(" :+that the optimization algorithm can find. If ");
126  addParamsLine(" :+the solution found is beyond this limit, then ");
127  addParamsLine(" :+the initial solution is returned instead of the ");
128  addParamsLine(" :+optimized one since this latter looks suspicious.");
129  addExampleLine("A typical use is:",false);
130  addExampleLine("xmipp_angular_continuous_assign -i anglesFromDiscreteAssignment.doc --ref reference.vol -o assigned_angles.xmd");
131 }
132 
133 // Produce side information ================================================
134 void ProgAngularContinuousAssign::preProcess()
135 {
136  // Read the reference volume
137  Image<double> V;
138  V.read(fn_ref);
139  V().setXmippOrigin();
140 
141  // Prepare the masks in real space
142  Mask mask_Real3D;
143  mask_Real3D.type = mask_Real.type = GAUSSIAN_MASK;
144  mask_Real3D.mode = mask_Real.mode = INNER_MASK;
145 
146  mask_Real3D.sigma = mask_Real.sigma = gaussian_Real_sigma * ((double)XSIZE(V()));
147 
148  mask_Real3D.generate_mask(V());
149  mask_Real.generate_mask(YSIZE(V()), XSIZE(V()));
150 
151  double gs2 = 2. * PI * gaussian_Real_sigma * gaussian_Real_sigma * ((double)XSIZE(V()) * (double) XSIZE(V()));
152  double gs3 = gs2 * sqrt(2. * PI) * gaussian_Real_sigma * ((double) XSIZE(V()));
153 
154  mask_Real3D.get_cont_mask() *= gs3;
155  mask_Real.get_cont_mask() *= gs2;
156 
157  //For debugging
158  /*Image<double> mask2D_test;
159  mask2D_test.read("ident2D.spi"); //Identity image
160  mask2D_test().setXmippOrigin();
161  mask_Real.apply_mask(mask2D_test(), mask2D_test());
162  mask2D_test.write("mask_2Dtest.xmp");*/
163 
164  // Prepare the weight function in Fourier space
165  mask_Fourier.type = GAUSSIAN_MASK;
166  mask_Fourier.mode = INNER_MASK;
167 
168  mask_Fourier.sigma = gaussian_DFT_sigma * ((double)XSIZE(V()));
169  mask_Fourier.generate_mask(YSIZE(V()), XSIZE(V()));
170 
171  double gsf2 = 2. * PI * gaussian_DFT_sigma * gaussian_DFT_sigma * ((double)XSIZE(V()) * (double)XSIZE(V()));
172  mask_Fourier.get_cont_mask() *= gsf2;
173  mask_Fourier.get_cont_mask()(0, 0) *= weight_zero_freq;
174 
175  // Weight the input volume in real space
176  mask_Real3D.apply_mask(V(), V());
177 
178  // Perform the DFT of the reference volume
179  int Status;
180  reDFTVolume = V();
181  imDFTVolume.resize(V());
182  CenterFFT(reDFTVolume, false);
183  VolumeDftRealToRealImaginary(MULTIDIM_ARRAY(reDFTVolume),
184  MULTIDIM_ARRAY(imDFTVolume), XSIZE(V()), YSIZE(V()), ZSIZE(V()),
185  &Status);
186  CenterFFT(reDFTVolume, true);
187  CenterFFT(imDFTVolume, true);
188 }
189 
190 // Predict =================================================================
191 void ProgAngularContinuousAssign::processImage(const FileName &fnImg, const FileName &fnImgOut, const MDRow &rowIn, MDRow &rowOut)
192 {
193  // Read the image and take its angles from the Metadata
194  // if they are available. If not, take them from the header.
195  // If not, set them to 0.
196  Image<double> img;
197  img.read(fnImg);
198  img().setXmippOrigin();
199 
200  double old_rot, old_tilt, old_psi, old_shiftX, old_shiftY;
201  rowIn.getValue(MDL_ANGLE_ROT,old_rot);
202  rowIn.getValue(MDL_ANGLE_TILT,old_tilt);
203  rowIn.getValue(MDL_ANGLE_PSI,old_psi);
204  rowIn.getValue(MDL_SHIFT_X,old_shiftX);
205  rowIn.getValue(MDL_SHIFT_Y,old_shiftY);
206 
207  Matrix1D<double> pose(5);
208  pose(0) = old_rot;
209  pose(1) = old_tilt;
210  pose(2) = old_psi;
211  pose(3) = -old_shiftX; // The convention of shifts is different
212  pose(4) = -old_shiftY; // for Slavica
213 
214  mask_Real.apply_mask(img(), img());
215 
216  double cost = CSTSplineAssignment(reDFTVolume, imDFTVolume,
217  img(), mask_Fourier.get_cont_mask(), pose, max_no_iter);
218 
219  Matrix2D<double> Eold, Enew;
220  Euler_angles2matrix(old_rot,old_tilt,old_psi,Eold);
221  Euler_angles2matrix(pose(0),pose(1),pose(2),Enew);
222  double angular_change=Euler_distanceBetweenMatrices(Eold,Enew);
223  double shift=sqrt(pose(3)*pose(3)+pose(4)*pose(4));
224  double new_rot=old_rot;
225  double new_tilt=old_tilt;
226  double new_psi=old_psi;
227  double new_shiftX=old_shiftX;
228  double new_shiftY=old_shiftY;
229  if (angular_change<max_angular_change || max_angular_change<0)
230  {
231  new_rot = pose(0);
232  new_tilt = pose(1);
233  new_psi = pose(2);
234  }
235  else
236  cost=-1;
237  if (shift<max_shift || max_shift<0)
238  {
239  new_shiftX = -pose(3);
240  new_shiftY = -pose(4);
241  }
242  else
243  cost=-1;
244 
245  rowOut.setValue(MDL_ANGLE_ROT, new_rot);
246  rowOut.setValue(MDL_ANGLE_TILT, new_tilt);
247  rowOut.setValue(MDL_ANGLE_PSI, new_psi);
248  rowOut.setValue(MDL_SHIFT_X, new_shiftX);
249  rowOut.setValue(MDL_SHIFT_Y, new_shiftY);
250  rowOut.setValue(MDL_COST, cost);
251 }
252 
253 /* ------------------------------------------------------------------------- */
254 /* Interface to Slavica's routines */
255 /* ------------------------------------------------------------------------- */
256 double CSTSplineAssignment(
257  MultidimArray<double> &ReDFTVolume,
258  MultidimArray<double> &ImDFTVolume,
259  MultidimArray<double> &image,
260  MultidimArray<double> &weight,
261  Matrix1D<double> &pose_parameters,
262  int max_no_iter
263 )
264 {
265  // Build the parameter structure .........................................
266  cstregistrationStruct Data;
267 
268  // Set the Volume input
269  Data.ReDftVolume = MULTIDIM_ARRAY(ReDFTVolume);
270  Data.nx_ReDftVolume = XSIZE(ReDFTVolume);
271  Data.ny_ReDftVolume = YSIZE(ReDFTVolume);
272  Data.nz_ReDftVolume = ZSIZE(ReDFTVolume);
273 
274  Data.ImDftVolume = MULTIDIM_ARRAY(ImDFTVolume);
275  Data.nx_ImDftVolume = XSIZE(ImDFTVolume);
276  Data.ny_ImDftVolume = YSIZE(ImDFTVolume);
277  Data.nz_ImDftVolume = ZSIZE(ImDFTVolume);
278 
279  // Perform the DFT of the input image
280  int Status;
281  MultidimArray<double> realImg(image), imagImg;
282  CenterFFT(realImg, false);
283  imagImg.resize(image);
284  VolumeDftRealToRealImaginary(MULTIDIM_ARRAY(realImg),
285  MULTIDIM_ARRAY(imagImg), XSIZE(image), YSIZE(image), 1, &Status);
286  CenterFFT(realImg, true);
287  CenterFFT(imagImg, true);
288 
289  // Set the Image input
290  Data.ReDftImage = MULTIDIM_ARRAY(realImg);
291  Data.nx_ReDftImage = XSIZE(realImg);
292  Data.ny_ReDftImage = YSIZE(realImg);
293 
294  Data.ImDftImage = MULTIDIM_ARRAY(imagImg);
295  Data.nx_ImDftImage = XSIZE(imagImg);
296  Data.ny_ImDftImage = YSIZE(imagImg);
297 
298  // Set the weight input
299  Data.Weight = MULTIDIM_ARRAY(weight);
300  Data.nx_Weight = XSIZE(weight);
301  Data.ny_Weight = YSIZE(weight);
302 
303  // Set the sampling rates
304  Matrix1D<double> sampling_rate(3);
305  sampling_rate.initConstant(1);
306  Data.VoxelSize = MATRIX1D_ARRAY(sampling_rate);
307  Data.nx_VoxelSize = 3;
308  Data.PixelSize = MATRIX1D_ARRAY(sampling_rate);
309  Data.nx_PixelSize = 2;
310 
311  // Set the initial pose parameters
312  Data.Parameters = MATRIX1D_ARRAY(pose_parameters);
313  Data.nx_Parameters = 5;
314 
315  // Set the final pose parameters.
316  Matrix2D<double> output_pose(5, max_no_iter + 1);
317  Data.OutputParameters = MATRIX2D_ARRAY(output_pose);
318  Data.nx_OutputParameters = max_no_iter + 1;
319  Data.ny_OutputParameters = 5;
320 
321  // Set performance parameters
322  Matrix1D<double> Cost, TimePerIter, Failures;
323  Cost.initZeros(max_no_iter + 1);
324  TimePerIter.initZeros(max_no_iter + 1);
325  Failures.initZeros(max_no_iter + 1);
326  long NumberIterPerformed, NumberSuccPerformed,
327  NumberFailPerformed;
328  Data.Cost = MATRIX1D_ARRAY(Cost);
329  Data.nx_Cost = max_no_iter + 1;
330  Data.TimePerIter = MATRIX1D_ARRAY(TimePerIter);
331  Data.nx_TimePerIter = max_no_iter + 1;
332  Data.NumberIterPerformed = &NumberIterPerformed;
333  Data.NumberSuccPerformed = &NumberSuccPerformed;
334  Data.NumberFailPerformed = &NumberFailPerformed;
335  Data.Failures = MATRIX1D_ARRAY(Failures);
336  Data.nx_Failures = max_no_iter + 1;
337 
338  // Set the parameters for the extracted central slice
339  MultidimArray<double> dftProj(2, YSIZE(image), XSIZE(image));
340  Data.dftProj = MULTIDIM_ARRAY(dftProj);
341  Data.nx_dftProj = XSIZE(image);
342  Data.ny_dftProj = YSIZE(image);
343  Data.nz_dftProj = 2;
344 
345  // Set the optimizer parameters
346  Data.ScaleLambda = 2;
347  Data.LambdaInitial = 1000;
348  Data.MaxNoIter = max_no_iter;
349  Data.MaxNoFailure = (long)(0.3 * max_no_iter);
350  Data.SatisfNoSuccess = (long)(0.7 * max_no_iter);
351  ;
352  Data.MakeDesiredProj = 0;
353  Data.ToleranceAngle = 0.0;
354  Data.ToleranceShift = 0.0;
355 
356  // Call Slavica's routine ...............................................
357  Status = cstregistration(&Data);
358 
359  // Retrieve results .....................................................
360  // Skip last iterations if they are failures
361  double retval;
362  if (Status != ERROR)
363  {
364  long last_iteration_performed = *(Data.NumberIterPerformed) + 1;
365  while (Failures(last_iteration_performed - 1) > 0.0)
366  last_iteration_performed--;
367 
368  // Get the pose parameters
369  pose_parameters(0) = RAD2DEG(output_pose(0, last_iteration_performed - 1));
370  pose_parameters(1) = RAD2DEG(output_pose(1, last_iteration_performed - 1));
371  pose_parameters(2) = RAD2DEG(output_pose(2, last_iteration_performed - 1));
372  pose_parameters(3) = output_pose(3, last_iteration_performed - 1);
373  pose_parameters(4) = output_pose(4, last_iteration_performed - 1);
374 
375  // Get the cost
376  retval = Cost(last_iteration_performed - 1);
377  }
378  else
379  {
380  retval = -1;
381  std::cout << "There is a problem with one image, angles not assigned\n";
382  }
383 
384  // Return
385  return retval;
386 }
387 
388 /* ------------------------------------------------------------------------- */
389 /* Boundary conditions */
390 /* ------------------------------------------------------------------------- */
391 long mirroring(long period, long k)
392 {
393  long sqk, qk;
394 
395  if (period == 1L)
396  qk = 0L;
397  else
398  qk = k - (2L * period - 2L) * (long) floor((double) k / (double)(2L * period - 2L));
399 
400  if ((qk < period) && (qk >= 0L))
401  sqk = qk;
402  else
403  sqk = 2L * period - 2L - qk;
404 
405  return sqk;
406 }
407 
408 long periodization(long period, long k)
409 {
410  long sqk, qk;
411 
412  if (period == 1L)
413  qk = 0L;
414  else if (k >= 0L)
415  qk = k - period * (long) floor((double) k / (double) period);
416  else
417  qk = ABS(k + 1L) - period * (long) floor((double) ABS(k + 1L) / (double) period);
418 
419  if ((k >= 0L) || (period == 1L))
420  sqk = qk;
421  else
422  sqk = period - 1L - qk;
423 
424  return sqk;
425 }
426 
427 #define PERIODIZATION(y, period, k) \
428 { \
429  long K=k; \
430  long qk; \
431  \
432  if (period == 1L) \
433  qk = 0L; \
434  else if (K >= 0L) \
435  qk = K - period * (long) floor((double) K / (double) period); \
436  else \
437  { \
438  long aux=ABS(K+1L); \
439  qk = aux - period * (long) floor((double) aux / (double) period); \
440  } \
441  \
442  if ((K >= 0L) || (period == 1L)) \
443  y = qk; \
444  else \
445  y = period - 1L - qk; \
446 }
447 
448 /* ------------------------------------------------------------------------- */
449 /* Gradient and Hessian at pixel */
450 /* ------------------------------------------------------------------------- */
451 int gradhesscost_atpixel(
452  double *Gradient,
453  double *Hessian,
454  double *cost,
455  double Difference,
456  double dp0,
457  double dp1,
458  double dp2,
459  double dp3,
460  double dp4,
461  double Weight)
462 {
463  long m, l, CC;
464 
465  *cost += Weight * Difference * Difference;
466 
467  for (m = 0L; m < 5L; m++)
468  {
469  CC = m * 5L;
470  if (m == 0L)
471  {
472  Gradient[0] += Weight * dp0 * Difference;
473  for (l = 0L; l < 5L; l++)
474  {
475  if (l == 0L)
476  {
477  Hessian[CC + l] += Weight * dp0 * dp0;
478  }
479  else if (l == 1L)
480  {
481  Hessian[CC + l] += Weight * dp0 * dp1;
482  }
483  else if (l == 2L)
484  {
485  Hessian[CC + l] += Weight * dp0 * dp2;
486  }
487  else if (l == 3L)
488  {
489  Hessian[CC + l] += Weight * dp0 * dp3;
490  }
491  else
492  {
493  Hessian[CC + l] += Weight * dp0 * dp4;
494  }
495 
496  }
497  }
498  if (m == 1L)
499  {
500  Gradient[1] += Weight * dp1 * Difference;
501  for (l = 1L; l < 5L; l++)
502  {
503  if (l == 1L)
504  {
505  Hessian[CC + l] += Weight * dp1 * dp1;
506  }
507  else if (l == 2L)
508  {
509  Hessian[CC + l] += Weight * dp1 * dp2;
510  }
511  else if (l == 3L)
512  {
513  Hessian[CC + l] += Weight * dp1 * dp3;
514  }
515  else
516  {
517  Hessian[CC + l] += Weight * dp1 * dp4;
518  }
519 
520  }
521  }
522  if (m == 2L)
523  {
524  Gradient[2] += Weight * dp2 * Difference;
525  for (l = 2L; l < 5L; l++)
526  {
527  if (l == 2L)
528  {
529  Hessian[CC + l] += Weight * dp2 * dp2;
530  }
531  else if (l == 3L)
532  {
533  Hessian[CC + l] += Weight * dp2 * dp3;
534  }
535  else
536  {
537  Hessian[CC + l] += Weight * dp2 * dp4;
538  }
539 
540  }
541  }
542  if (m == 3L)
543  {
544  Gradient[3] += Weight * dp3 * Difference;
545  for (l = 3L; l < 5L; l++)
546  {
547  if (l == 3L)
548  {
549  Hessian[CC + l] += Weight * dp3 * dp3;
550  }
551  else
552  {
553  Hessian[CC + l] += Weight * dp3 * dp4;
554  }
555 
556  }
557  }
558  if (m == 4L)
559  {
560  Gradient[4] += Weight * dp4 * Difference;
561  Hessian[CC + 4L] += Weight * dp4 * dp4;
562  }
563  }
564 
565  return(!ERROR);
566 }/* End of gradhesscost_atpixel */
567 
568 /* ------------------------------------------------------------------------- */
569 /* Optimizer */
570 /* ------------------------------------------------------------------------- */
571 int return_gradhesscost(
572  double *Gradient,
573  double *Hessian,
574  double *cost,
575  double *Parameters,
576  double *CoefRe,
577  double *CoefIm,
578  double *pntr_ReInp,
579  double *pntr_ImInp,
580  double *pntr_Weight,
581  long Nx,
582  long Ny,
583  long Nz,
584  long indx0,
585  long indy0,
586  long indz0,
587  long Mx,
588  long My,
589  double *Left,
590  double *Right,
591  double *Q1,
592  double *Q3,
593  double scx1,
594  double scy1,
595  double S,
596  long SizeIm,
597  int DoDesProj,
598  double *dftCompProj)
599  {
600  int Status = !ERROR;
601  long indx, indy, l, l1, l2, m, m1, m2, n, n1, n2;
602  long i, j, CC1, CC, index;
603  double phi, theta, psi, x0, y0, Sinphi, Cosphi, Sinpsi, Cospsi, Sintheta, Costheta;
604  double *hlp, *Rz1, *Ry, *Rz2, *DRz1, *DRy, *DRz2;
605  double *DR0, *DR1, *DR2, *mn, *Arg;
606  double *Grad_re, *Grad_im, *Hessian_re, *Hessian_im, *Gradient_re, *Gradient_im;
607  double scx, scy, x, y, z;
608  double xminusl, yminusm, zminusn, re_Coeff, im_Coeff;
609  double re_columns, re_rows, im_columns, im_rows;
610  double re_slices, re_slices_d1, re_slices_d2, re_slices_d3, re_columns_d1, re_columns_d2, re_rows_d1;
611  double im_slices, im_slices_d1, im_slices_d2, im_slices_d3, im_columns_d1, im_columns_d2, im_rows_d1;
612  double a, b, c, SinC, CosC, redftproj, imdftproj;
613  double da, db, dc, da0[1], da1[1], da2[1], da3[1], da4[1];
614  double db0[1], db1[1], db2[1], db3[1], db4[1], dc0[1], dc1[1], dc2[1], dc3[1], dc4[1];
615  double prv_re, prv_im, Difference_re, Difference_im, Weight;
616  double dp0_re, dp0_im, dp1_re, dp1_im, dp2_re, dp2_im, dp3_re, dp3_im, dp4_re, dp4_im;
617  double cost_re, cost_im, *R, *Q, *Q2, *dQ0, *dQ1, *dQ2, *dQ2_0, *dQ2_1, *dQ2_2;
618  double sum_xx_re, sum_xx_im, sc_re, sc_im;
619  double *DP_0, *DP_1, *DP_2, *DP_3, *DP_4, *pntr_ReOut, *pntr_ImOut;
620  double *pntr_DP_re, *pntr_DP_im, *pntr_DP_0_re, *pntr_DP_0_im, *pntr_DP_1_re, *pntr_DP_1_im;
621  double *pntr_DP_2_re, *pntr_DP_2_im, *pntr_DP_3_re, *pntr_DP_3_im;
622  double *pntr_DP_4_re, *pntr_DP_4_im;
623 
624  size_t poolSize = (18 * 16) + (4 * 4) + (2 * 5) + (2 * 25);
625  auto pool = std::unique_ptr<double[]>(new double[poolSize]);
626  double *poolNext = pool.get();
627  auto getNextFromPool = [&](size_t count) {
628  auto tmp = poolNext;
629  poolNext += count;
630  return tmp;
631  };
632 
633  if (( ! pool) || (nullptr == poolNext)) {
634  WRITE_ERROR(return_gradhesscost, "ERROR - Not enough memory for memory pool");
635  return(ERROR);
636  }
637 
638  phi = Parameters[0];
639  theta = Parameters[1];
640  psi = Parameters[2];
641  x0 = Parameters[3];
642  y0 = Parameters[4];
643 
644  Sinphi = sin(phi);
645  Cosphi = cos(phi);
646  Sinpsi = sin(psi);
647  Cospsi = cos(psi);
648  Sintheta = sin(theta);
649  Costheta = cos(theta);
650 
651  Rz1 = getNextFromPool(16);
652  Ry = getNextFromPool(16);
653  Rz2 = getNextFromPool(16);
654  if (GetIdentitySquareMatrix(Rz2, 4L) == ERROR)
655  {
656  WRITE_ERROR(return_gradhesscost, "Error returned by GetIdentitySquareMatrix");
657  return(ERROR);
658  }
659 
660  hlp = Rz2;
661  *hlp++ = Cospsi;
662  *hlp = Sinpsi;
663  hlp += (std::ptrdiff_t)3L;
664  *hlp++ = - Sinpsi;
665  *hlp = Cospsi;
666 
667  if (GetIdentitySquareMatrix(Rz1, 4L) == ERROR)
668  {
669  WRITE_ERROR(return_gradhesscost, "Error returned by GetIdentitySquareMatrix");
670  return(ERROR);
671  }
672 
673  hlp = Rz1;
674  *hlp++ = Cosphi;
675  *hlp = Sinphi;
676  hlp += (std::ptrdiff_t)3L;
677  *hlp++ = - Sinphi;
678  *hlp = Cosphi;
679 
680  if (GetIdentitySquareMatrix(Ry, 4L) == ERROR)
681  {
682  WRITE_ERROR(return_gradhesscost, "Error returned by GetIdentitySquareMatrix");
683  return(ERROR);
684  }
685 
686  hlp = Ry;
687  *hlp = Costheta;
688  hlp += (std::ptrdiff_t)2L;
689  *hlp = - Sintheta;
690  hlp += (std::ptrdiff_t)6L;
691  *hlp = Sintheta;
692  hlp += (std::ptrdiff_t)2L;
693  *hlp = Costheta;
694 
695  R = getNextFromPool(16);
696  if (multiply_3Matrices(Rz2, Ry, Rz1, R, 4L, 4L, 4L, 4L) == ERROR)
697  {
698  WRITE_ERROR(return_gradhesscost, "Error returned by multiply_3Matrices");
699  return(ERROR);
700  }
701 
702  DRz1 = getNextFromPool(16);
703  DRy = getNextFromPool(16);
704  DRz2 = getNextFromPool(16);
705  for (i = 0L; i < 16L; i++)
706  {
707  DRz2[i] = 0.0;
708  DRz1[i] = 0.0;
709  DRy[i] = 0.0;
710  }
711 
712  hlp = DRz2;
713  *hlp++ = - Sinpsi;
714  *hlp = Cospsi;
715  hlp += (std::ptrdiff_t)3L;
716  *hlp++ = - Cospsi;
717  *hlp = - Sinpsi;
718 
719  hlp = DRz1;
720  *hlp++ = - Sinphi;
721  *hlp = Cosphi;
722  hlp += (std::ptrdiff_t)3L;
723  *hlp++ = - Cosphi;
724  *hlp = - Sinphi;
725 
726  hlp = DRy;
727  *hlp = - Sintheta;
728  hlp += (std::ptrdiff_t)2L;
729  *hlp = - Costheta;
730  hlp += (std::ptrdiff_t)6L;
731  *hlp = Costheta;
732  hlp += (std::ptrdiff_t)2L;
733  *hlp = - Sintheta;
734 
735  DR0 = getNextFromPool(16);
736  DR1 = getNextFromPool(16);
737  DR2 = getNextFromPool(16);
738  if (multiply_3Matrices(Rz2, Ry, DRz1, DR0, 4L, 4L, 4L, 4L) == ERROR)
739  {
740  WRITE_ERROR(return_gradhesscost, "Error returned by multiply_3Matrices");
741  return(ERROR);
742  }
743  if (multiply_3Matrices(Rz2, DRy, Rz1, DR1, 4L, 4L, 4L, 4L) == ERROR)
744  {
745  WRITE_ERROR(return_gradhesscost, "Error returned by multiply_3Matrices");
746  return(ERROR);
747  }
748  if (multiply_3Matrices(DRz2, Ry, Rz1, DR2, 4L, 4L, 4L, 4L) == ERROR)
749  {
750  WRITE_ERROR(return_gradhesscost, "Error returned by multiply_3Matrices");
751  return(ERROR);
752  }
753 
754  mn = getNextFromPool(4);
755  Arg = getNextFromPool(4);
756  Grad_re = getNextFromPool(4);
757  Grad_im = getNextFromPool(4);
758  Gradient_re = getNextFromPool(5);
759  Gradient_im = getNextFromPool(5);
760  Hessian_re = getNextFromPool(25);
761  Hessian_im = getNextFromPool(25);
762  AllocateVolumeDouble(&DP_0, Mx, My, 2L, &Status);
763  if (Status == ERROR)
764  {
765  WRITE_ERROR(return_gradhesscost, "ERROR - Not enough memory for DP_0");
766  return(ERROR);
767  }
768  AllocateVolumeDouble(&DP_1, Mx, My, 2L, &Status);
769  if (Status == ERROR)
770  {
771  WRITE_ERROR(return_gradhesscost, "ERROR - Not enough memory for DP_1");
772  FreeVolumeDouble(&DP_0);
773  return(ERROR);
774  }
775  AllocateVolumeDouble(&DP_2, Mx, My, 2L, &Status);
776  if (Status == ERROR)
777  {
778  WRITE_ERROR(return_gradhesscost, "ERROR - Not enough memory for DP_2");
779  FreeVolumeDouble(&DP_0);
780  FreeVolumeDouble(&DP_1);
781  return(ERROR);
782  }
783  AllocateVolumeDouble(&DP_3, Mx, My, 2L, &Status);
784  if (Status == ERROR)
785  {
786  WRITE_ERROR(return_gradhesscost, "ERROR - Not enough memory for DP_3");
787  FreeVolumeDouble(&DP_0);
788  FreeVolumeDouble(&DP_1);
789  FreeVolumeDouble(&DP_2);
790  return(ERROR);
791  }
792  AllocateVolumeDouble(&DP_4, Mx, My, 2L, &Status);
793  if (Status == ERROR)
794  {
795  WRITE_ERROR(return_gradhesscost, "ERROR - Not enough memory for DP_4");
796  FreeVolumeDouble(&DP_0);
797  FreeVolumeDouble(&DP_1);
798  FreeVolumeDouble(&DP_2);
799  FreeVolumeDouble(&DP_3);
800  return(ERROR);
801  }
802 
803  pntr_ReOut = dftCompProj;
804  pntr_ImOut = pntr_ReOut + (std::ptrdiff_t) SizeIm;
805 
806  pntr_DP_0_re = DP_0;
807  pntr_DP_0_im = pntr_DP_0_re + (std::ptrdiff_t) SizeIm;
808 
809  pntr_DP_1_re = DP_1;
810  pntr_DP_1_im = pntr_DP_1_re + (std::ptrdiff_t) SizeIm;
811 
812  pntr_DP_2_re = DP_2;
813  pntr_DP_2_im = pntr_DP_2_re + (std::ptrdiff_t) SizeIm;
814 
815  pntr_DP_3_re = DP_3;
816  pntr_DP_3_im = pntr_DP_3_re + (std::ptrdiff_t) SizeIm;
817 
818  pntr_DP_4_re = DP_4;
819  pntr_DP_4_im = pntr_DP_4_re + (std::ptrdiff_t) SizeIm;
820 
821  Q = getNextFromPool(16);
822  Q2 = getNextFromPool(16);
823  auto freeAllVolumes = [&] {
824  FreeVolumeDouble(&DP_0);
825  FreeVolumeDouble(&DP_1);
826  FreeVolumeDouble(&DP_2);
827  FreeVolumeDouble(&DP_3);
828  FreeVolumeDouble(&DP_4);
829  };
830 
831  if (multiply_3Matrices(Left, R, Right, Q, 4L, 4L, 4L, 4L) == ERROR)
832  {
833  WRITE_ERROR(return_gradhesscost, "Error returned by multiply_3Matrices");
834  freeAllVolumes();
835  return(ERROR);
836  }
837  if (MatrixTranspose(Q, Q2, 4L, 4L) == ERROR)
838  {
839  WRITE_ERROR(return_gradhesscost, "Error returned by MatrixTranspose");
840  freeAllVolumes();
841  return(ERROR);
842  }
843 
844  if (multiply_3Matrices(Q1, Q2, Q3, Q, 4L, 4L, 4L, 4L) == ERROR)
845  {
846  WRITE_ERROR(return_gradhesscost, "Error returned by multiply_3Matrices");
847  freeAllVolumes();
848  return(ERROR);
849  }
850 
851 
852  dQ0 = getNextFromPool(16);
853  dQ1 = getNextFromPool(16);
854  dQ2 = getNextFromPool(16);
855  dQ2_0 = getNextFromPool(16);
856  dQ2_1 = getNextFromPool(16);
857  dQ2_2 = getNextFromPool(16);
858  if (multiply_3Matrices(Left, DR0, Right, dQ0, 4L, 4L, 4L, 4L) == ERROR)
859  {
860  WRITE_ERROR(return_gradhesscost, "Error returned by multiply_3Matrices");
861  freeAllVolumes();
862  return(ERROR);
863  }
864  if (MatrixTranspose(dQ0, dQ2_0, 4L, 4L) == ERROR)
865  {
866  WRITE_ERROR(return_gradhesscost, "Error returned by MatrixTranspose");
867  freeAllVolumes();
868  return(ERROR);
869  }
870  if (multiply_3Matrices(Left, DR1, Right, dQ1, 4L, 4L, 4L, 4L) == ERROR)
871  {
872  WRITE_ERROR(return_gradhesscost, "Error returned by multiply_3Matrices");
873  freeAllVolumes();
874  return(ERROR);
875  }
876  if (MatrixTranspose(dQ1, dQ2_1, 4L, 4L) == ERROR)
877  {
878  WRITE_ERROR(return_gradhesscost, "Error returned by MatrixTranspose");
879  freeAllVolumes();
880  return(ERROR);
881  }
882  if (multiply_3Matrices(Left, DR2, Right, dQ2, 4L, 4L, 4L, 4L) == ERROR)
883  {
884  WRITE_ERROR(return_gradhesscost, "Error returned by multiply_3Matrices");
885  freeAllVolumes();
886  return(ERROR);
887  }
888  if (MatrixTranspose(dQ2, dQ2_2, 4L, 4L) == ERROR)
889  {
890  WRITE_ERROR(return_gradhesscost, "Error returned by MatrixTranspose");
891  freeAllVolumes();
892  return(ERROR);
893  }
894 
895 
896  if (multiply_3Matrices(Q1, dQ2_0, Q3, dQ0, 4L, 4L, 4L, 4L) == ERROR)
897  {
898  WRITE_ERROR(return_gradhesscost, "Error returned by multiply_3Matrices");
899  freeAllVolumes();
900  return(ERROR);
901  }
902 
903  if (multiply_3Matrices(Q1, dQ2_1, Q3, dQ1, 4L, 4L, 4L, 4L) == ERROR)
904  {
905  WRITE_ERROR(return_gradhesscost, "Error returned by multiply_3Matrices");
906  freeAllVolumes();
907  return(ERROR);
908  }
909 
910  if (multiply_3Matrices(Q1, dQ2_2, Q3, dQ2, 4L, 4L, 4L, 4L) == ERROR)
911  {
912  WRITE_ERROR(return_gradhesscost, "Error returned by multiply_3Matrices");
913  freeAllVolumes();
914  return(ERROR);
915  }
916 
917 
918  sum_xx_re = 0.0;
919  sum_xx_im = 0.0;
920 
921  scx = scx1 * x0;
922  scy = scy1 * y0;
923 
924  double aux, aux2;
925  for (indy = 0L; indy < My; indy++)
926  {
927  for (indx = 0L; indx < Mx; indx++)
928  {
929 
930  mn[0] = (double)(indx - Mx / 2L);
931  mn[1] = (double)(indy - My / 2L);
932  mn[2] = 0.0;
933  mn[3] = 1.0;
934 
935  if (MatrixTimesVector(Q, mn, Arg, 4L, 4L) == ERROR)
936  {
937  WRITE_ERROR(return_gradhesscost, "Error returned by MatrixTimesVector");
938  freeAllVolumes();
939  return(ERROR);
940  }
941 
942  x = Arg[0];
943  y = Arg[1];
944  z = Arg[2];
945 
946  l1 = (long) ceil(x - 2.0);
947  l2 = l1 + 3L;
948  m1 = (long) ceil(y - 2.0);
949  m2 = m1 + 3L;
950  n1 = (long) ceil(z - 2.0);
951  n2 = n1 + 3L;
952 
953  re_slices = 0.0;
954  re_slices_d1 = 0.0;
955  re_slices_d2 = 0.0;
956  re_slices_d3 = 0.0;
957  im_slices = 0.0;
958  im_slices_d1 = 0.0;
959  im_slices_d2 = 0.0;
960  im_slices_d3 = 0.0;
961  for (n = n1; n <= n2; n++)
962  {
963  long Laux;
964  PERIODIZATION(Laux,Nz, n - indz0);
965  CC1 = Nx * Ny * Laux;
966  re_columns = 0.0;
967  re_columns_d1 = 0.0;
968  re_columns_d2 = 0.0;
969  im_columns = 0.0;
970  im_columns_d1 = 0.0;
971  im_columns_d2 = 0.0;
972  for (m = m1; m <= m2; m++)
973  {
974  PERIODIZATION(Laux,Ny, m - indy0);
975  CC = CC1 + Nx * Laux;
976  re_rows = 0.0;
977  re_rows_d1 = 0.0;
978  im_rows = 0.0;
979  im_rows_d1 = 0.0;
980  for (l = l1; l <= l2; l++)
981  {
982  xminusl = x - (double) l;
983  PERIODIZATION(Laux,Nx, l - indx0);
984  long Laux2=CC + Laux;
985  re_Coeff = CoefRe[Laux2];
986  im_Coeff = CoefIm[Laux2];
987  BSPLINE03(aux,xminusl);
988  BSPLINE03DIFF1(aux2,xminusl);
989  re_rows += re_Coeff * aux;
990  re_rows_d1 += re_Coeff * aux2;
991  im_rows += im_Coeff * aux;
992  im_rows_d1 += im_Coeff * aux2;
993  }
994  yminusm = y - (double) m;
995  BSPLINE03(aux,yminusm);
996  BSPLINE03DIFF1(aux2,yminusm);
997  re_columns += re_rows * aux;
998  re_columns_d1 += re_rows_d1 * aux;
999  re_columns_d2 += re_rows * aux2;
1000  im_columns += im_rows * aux;
1001  im_columns_d1 += im_rows_d1 * aux;
1002  im_columns_d2 += im_rows * aux2;
1003  }
1004  zminusn = z - (double) n;
1005  BSPLINE03(aux,zminusn);
1006  BSPLINE03DIFF1(aux2,zminusn);
1007  re_slices += re_columns * aux;
1008  re_slices_d1 += re_columns_d1 * aux;
1009  re_slices_d2 += re_columns_d2 * aux;
1010  re_slices_d3 += re_columns * aux2;
1011  im_slices += im_columns * aux;
1012  im_slices_d1 += im_columns_d1 * aux;
1013  im_slices_d2 += im_columns_d2 * aux;
1014  im_slices_d3 += im_columns * aux2;
1015  }
1016 
1017  a = re_slices;
1018  Grad_re[0] = re_slices_d1;
1019  Grad_re[1] = re_slices_d2;
1020  Grad_re[2] = re_slices_d3;
1021  Grad_re[3] = 0.0;
1022 
1023  b = im_slices;
1024  Grad_im[0] = im_slices_d1;
1025  Grad_im[1] = im_slices_d2;
1026  Grad_im[2] = im_slices_d3;
1027  Grad_im[3] = 0.0;
1028 
1029  c = scx * mn[0] + scy * mn[1];
1030 
1031  SinC = sin(c);
1032  CosC = cos(c);
1033 
1034  redftproj = (a * CosC + b * SinC) * S;
1035  imdftproj = (b * CosC - a * SinC) * S;
1036 
1037  index = indy * Mx + indx;
1038 
1039  pntr_ReOut[index] = redftproj;
1040  pntr_ImOut[index] = imdftproj;
1041 
1042 
1043  if (!((indx == (Mx / 2L)) && (indy == (My / 2L))))
1044  {
1045  sum_xx_re += redftproj * redftproj;
1046  sum_xx_im += imdftproj * imdftproj;
1047  }
1048 
1049  if (MatrixTimesVector(dQ0, mn, Arg, 4L, 4L) == ERROR)
1050  {
1051  WRITE_ERROR(return_gradhesscost, "Error returned by MatrixTimesVector");
1052  freeAllVolumes();
1053  return(ERROR);
1054  }
1055  if (VectorScalarProduct(Grad_re, Arg, da0, 4L) == ERROR)
1056  {
1057  WRITE_ERROR(return_gradhesscost, "Error returned by VectorScalarProduct");
1058  freeAllVolumes();
1059  return(ERROR);
1060  }
1061  if (VectorScalarProduct(Grad_im, Arg, db0, 4L) == ERROR)
1062  {
1063  WRITE_ERROR(return_gradhesscost, "Error returned by VectorScalarProduct");
1064  freeAllVolumes();
1065  return(ERROR);
1066  }
1067 
1068  if (MatrixTimesVector(dQ1, mn, Arg, 4L, 4L) == ERROR)
1069  {
1070  WRITE_ERROR(return_gradhesscost, "Error returned by MatrixTimesVector");
1071  freeAllVolumes();
1072  return(ERROR);
1073  }
1074  if (VectorScalarProduct(Grad_re, Arg, da1, 4L) == ERROR)
1075  {
1076  WRITE_ERROR(return_gradhesscost, "Error returned by VectorScalarProduct");
1077  freeAllVolumes();
1078  return(ERROR);
1079  }
1080  if (VectorScalarProduct(Grad_im, Arg, db1, 4L) == ERROR)
1081  {
1082  WRITE_ERROR(return_gradhesscost, "Error returned by VectorScalarProduct");
1083  freeAllVolumes();
1084  return(ERROR);
1085  }
1086 
1087  if (MatrixTimesVector(dQ2, mn, Arg, 4L, 4L) == ERROR)
1088  {
1089  WRITE_ERROR(return_gradhesscost, "Error returned by MatrixTimesVector");
1090  freeAllVolumes();
1091  return(ERROR);
1092  }
1093  if (VectorScalarProduct(Grad_re, Arg, da2, 4L) == ERROR)
1094  {
1095  WRITE_ERROR(return_gradhesscost, "Error returned by VectorScalarProduct");
1096  freeAllVolumes();
1097  return(ERROR);
1098  }
1099  if (VectorScalarProduct(Grad_im, Arg, db2, 4L) == ERROR)
1100  {
1101  WRITE_ERROR(return_gradhesscost, "Error returned by VectorScalarProduct");
1102  freeAllVolumes();
1103  return(ERROR);
1104  }
1105 
1106  da3[0] = 0.0;
1107  db3[0] = 0.0;
1108  da4[0] = 0.0;
1109  db4[0] = 0.0;
1110 
1111  dc0[0] = 0.0;
1112  dc1[0] = 0.0;
1113  dc2[0] = 0.0;
1114 
1115  dc3[0] = scx1 * mn[0];
1116  dc4[0] = scy1 * mn[1];
1117 
1118  da = da0[0];
1119  dc = dc0[0];
1120  db = db0[0];
1121  pntr_DP_re = pntr_DP_0_re;
1122  pntr_DP_im = pntr_DP_0_im;
1123  pntr_DP_re[index] =
1124  (da * CosC - a * SinC * dc + db * SinC + b * CosC * dc) * S;
1125  pntr_DP_im[index] =
1126  (db * CosC - b * SinC * dc - da * SinC - a * CosC * dc) * S;
1127 
1128 
1129  da = da1[0];
1130  dc = dc1[0];
1131  db = db1[0];
1132  pntr_DP_re = pntr_DP_1_re;
1133  pntr_DP_im = pntr_DP_1_im;
1134  pntr_DP_re[index] =
1135  (da * CosC - a * SinC * dc + db * SinC + b * CosC * dc) * S;
1136  pntr_DP_im[index] =
1137  ((db * CosC - b * SinC * dc - da * SinC - a * CosC * dc) * S);
1138 
1139  da = da2[0];
1140  dc = dc2[0];
1141  db = db2[0];
1142  pntr_DP_re = pntr_DP_2_re;
1143  pntr_DP_im = pntr_DP_2_im;
1144  pntr_DP_re[index] =
1145  ((da * CosC - a * SinC * dc + db * SinC + b * CosC * dc) * S);
1146  pntr_DP_im[index] =
1147  ((db * CosC - b * SinC * dc - da * SinC - a * CosC * dc) * S);
1148 
1149 
1150  da = da3[0];
1151  dc = dc3[0];
1152  db = db3[0];
1153  pntr_DP_re = pntr_DP_3_re;
1154  pntr_DP_im = pntr_DP_3_im;
1155  pntr_DP_re[index] =
1156  ((da * CosC - a * SinC * dc + db * SinC + b * CosC * dc) * S);
1157  pntr_DP_im[index] =
1158  ((db * CosC - b * SinC * dc - da * SinC - a * CosC * dc) * S);
1159 
1160  da = da4[0];
1161  dc = dc4[0];
1162  db = db4[0];
1163  pntr_DP_re = pntr_DP_4_re;
1164  pntr_DP_im = pntr_DP_4_im;
1165  pntr_DP_re[index] =
1166  ((da * CosC - a * SinC * dc + db * SinC + b * CosC * dc) * S);
1167  pntr_DP_im[index] =
1168  ((db * CosC - b * SinC * dc - da * SinC - a * CosC * dc) * S);
1169 
1170  }
1171  }
1172 
1173  if (!DoDesProj)
1174  {
1175 
1176  cost_re = 0.0;
1177  cost_im = 0.0;
1178  for (i = 0L; i < 5L; i++)
1179  {
1180  Gradient_re[i] = 0.0;
1181  Gradient_im[i] = 0.0;
1182  for (j = 0L; j < 5L; j++)
1183  {
1184  Hessian_re[i * 5L + j] = 0.0;
1185  Hessian_im[i * 5L + j] = 0.0;
1186  }
1187  }
1188 
1189  sc_re = 1.0 / sqrt(sum_xx_re + sum_xx_im);
1190  sc_im = sc_re;
1191 
1192 
1193  for (indy = 0L; indy < My; indy++)
1194  {
1195  for (indx = 0L; indx < Mx; indx++)
1196  {
1197 
1198  index = indy * Mx + indx;
1199 
1200  Weight = (double) pntr_Weight[index];
1201 
1202  prv_re = (sc_re * (double) pntr_ReOut[index]);
1203  Difference_re = (double)(prv_re - pntr_ReInp[index]);
1204 
1205  prv_im = (sc_im * (double) pntr_ImOut[index]);
1206  Difference_im = (double)(prv_im - pntr_ImInp[index]);
1207 
1208  dp0_re = sc_re * (double) pntr_DP_0_re[index];
1209  dp1_re = sc_re * (double) pntr_DP_1_re[index];
1210  dp2_re = sc_re * (double) pntr_DP_2_re[index];
1211  dp3_re = sc_re * (double) pntr_DP_3_re[index];
1212  dp4_re = sc_re * (double) pntr_DP_4_re[index];
1213 
1214  dp0_im = sc_im * (double) pntr_DP_0_im[index];
1215  dp1_im = sc_im * (double) pntr_DP_1_im[index];
1216  dp2_im = sc_im * (double) pntr_DP_2_im[index];
1217  dp3_im = sc_im * (double) pntr_DP_3_im[index];
1218  dp4_im = sc_im * (double) pntr_DP_4_im[index];
1219 
1220  if (gradhesscost_atpixel(Gradient_re, Hessian_re, &cost_re, Difference_re, dp0_re, dp1_re, dp2_re, dp3_re, dp4_re, Weight) == ERROR)
1221  {
1222  WRITE_ERROR(return_gradhesscost, "Error returned by gradhesscost_atpixel");
1223  freeAllVolumes();
1224  return(ERROR);
1225  }
1226  if (gradhesscost_atpixel(Gradient_im, Hessian_im, &cost_im, Difference_im, dp0_im, dp1_im, dp2_im, dp3_im, dp4_im, Weight))
1227  {
1228  WRITE_ERROR(return_gradhesscost, "Error returned by gradhesscost_atpixel");
1229  freeAllVolumes();
1230  return(ERROR);
1231  }
1232 
1233  }
1234  }
1235 
1236 
1237  *cost = (cost_re + cost_im) / 2.0;
1238 
1239  for (i = 0L; i < 5L; i++)
1240  {
1241  Gradient[i] = Gradient_re[i] + Gradient_im[i];
1242  for (j = 0L; j < 5L; j++)
1243  Hessian[i * 5L + j] = Hessian_re[i * 5L + j] + Hessian_im[i * 5L + j];
1244  }
1245 
1246 
1247  for (i = 0L; i < 4L; i++)
1248  for (j = i + 1L; j < 5L; j++)
1249  Hessian[j * 5L + i] = Hessian[i * 5L + j];
1250 
1251  }
1252 
1253  freeAllVolumes();
1254  return(!ERROR);
1255  }/* End of return_gradhesscost */
1256 
1257  /* ------------------------------------------------------------------------- */
1258  /* Optimizer */
1259  /* ------------------------------------------------------------------------- */
1260  int levenberg_cst(
1261  double beta[],
1262  double *alpha,
1263  double *cost,
1264  double a[],
1265  double *CoefRe,
1266  double *CoefIm,
1267  double *pntr_ReInp,
1268  double *pntr_ImInp,
1269  double *pntr_Weight,
1270  long Nx,
1271  long Ny,
1272  long Nz,
1273  long indx0,
1274  long indy0,
1275  long indz0,
1276  long Mx,
1277  long My,
1278  double *Left,
1279  double *Right,
1280  double *Q1,
1281  double *Q3,
1282  double scx1,
1283  double scy1,
1284  double S,
1285  long SizeIm,
1286  int DoDesProj,
1287  double *dftCompProj,
1288  double OldCost,
1289  double *lambda,
1290  double LambdaScale,
1291  long *iter,
1292  double tol_angle,
1293  double tol_shift,
1294  int *IteratingStop)
1295  {
1296 #ifndef DBL_EPSILON
1297 #define DBL_EPSILON 2e-16
1298 #endif
1299  double *da, epsilon = DBL_EPSILON;
1300  double *u, *v, *w;
1301  double *t;
1302  double wmax, thresh;
1303  long i, j, ma = 5L;
1304 
1305  double costchanged[1];
1306 
1307  da = (double *)malloc((size_t)ma * sizeof(double));
1308  if (da == (double *)NULL)
1309  {
1310  WRITE_ERROR(levenberg_cst, "ERROR - Not enough memory for da in levenberg_cst");
1311  return(ERROR);
1312  }
1313  u = (double *)malloc((size_t)(ma * ma) * sizeof(double));
1314  if (u == (double *)NULL)
1315  {
1316  free(da);
1317  WRITE_ERROR(levenberg_cst, "ERROR - Not enough memory for u in levenberg_cst");
1318  return(ERROR);
1319  }
1320  v = (double *)malloc((size_t)(ma * ma) * sizeof(double));
1321  if (v == (double *)NULL)
1322  {
1323  free(u);
1324  free(da);
1325  WRITE_ERROR(levenberg_cst, "ERROR - Not enough memory for v in levenberg_cst");
1326  return(ERROR);
1327  }
1328  w = (double *)malloc((size_t)ma * sizeof(double));
1329  if (w == (double *)NULL)
1330  {
1331  free(v);
1332  free(u);
1333  free(da);
1334  WRITE_ERROR(levenberg_cst, "ERROR - Not enough memory for w in levenberg_cst");
1335  return(ERROR);
1336  }
1337 
1338 
1339  t = u;
1340  for (i = 0L; (i < ma); t += (std::ptrdiff_t)(ma + 1L), i++)
1341  {
1342  for (j = 0L; (j < ma); alpha++, j++)
1343  *u++ = -*alpha;
1344  *t *= 1.0 + *lambda;
1345  }
1346  u -= (std::ptrdiff_t)(ma * ma);
1347  alpha -= (std::ptrdiff_t)(ma * ma);
1348 
1349  int Status;
1350  if (SingularValueDecomposition(u, ma, ma, w, v, SVDMAXITER, &Status) == ERROR)
1351  {
1352  free(w);
1353  free(v);
1354  free(u);
1355  free(da);
1356  WRITE_ERROR(levenberg_cst, "ERROR - Unable to perform svdcmp in levenberg_cst");
1357  return(ERROR);
1358  }
1359  wmax = 0.0;
1360  t = w + (std::ptrdiff_t)ma;
1361  while (--t >= w)
1362  if (*t > wmax)
1363  wmax = *t;
1364  thresh = epsilon * wmax;
1365  w += (std::ptrdiff_t)ma;
1366  j = ma;
1367  while (--j >= 0L)
1368  {
1369  if (*--w < thresh)
1370  {
1371  *w = 0.0;
1372  for (i = 0; (i < ma); i++)
1373  {
1374  u[i * ma + j] = 0.0;
1375  v[i * ma + j] = 0.0;
1376  }
1377  }
1378  }
1379  if (SingularValueBackSubstitution(u, w, v, ma, ma, beta, da, &Status) == ERROR)
1380  {
1381  free(w);
1382  free(v);
1383  free(u);
1384  free(da);
1385  WRITE_ERROR(levenberg_cst, "ERROR - Unable to perform svbksb in levenberg_cst");
1386  return(ERROR);
1387  }
1388 
1389 
1390  v = (double *)memcpy(v, a, (size_t)ma * sizeof(double));
1391  t = v + (std::ptrdiff_t)ma;
1392  a += (std::ptrdiff_t)ma;
1393  da += (std::ptrdiff_t)ma;
1394  while (--t >= v)
1395  {
1396  da--;
1397  *t = *--a;
1398  *t += *da;
1399  }
1400 
1401 
1402  if (return_gradhesscost(w, u, costchanged, v,
1403  CoefRe, CoefIm, pntr_ReInp, pntr_ImInp, pntr_Weight,
1404  Nx, Ny, Nz, indx0, indy0, indz0, Mx, My,
1405  Left, Right, Q1, Q3, scx1, scy1, S, SizeIm,
1406  DoDesProj, dftCompProj) == ERROR)
1407  {
1408  WRITE_ERROR(levenberg_cst, "Error returned by total_gradhesscost");
1409  free(w);
1410  free(v);
1411  free(u);
1412  free(da);
1413  return(ERROR);
1414  }
1415 
1416 
1417  (*iter)++;
1418  if (costchanged[0] < OldCost)
1419  {
1420  if ((fabs(a[0] - v[0]) < tol_angle) && (fabs(a[1] - v[1]) < tol_angle) && (fabs(a[2] - v[2]) < tol_angle))
1421  {
1422  if ((fabs(a[3] - v[3]) < tol_shift) && (fabs(a[4] - v[4]) < tol_shift))
1423  {
1424  *IteratingStop = 1;
1425  }
1426  }
1427  }
1428 
1429  if (*cost == -1.0)
1430  {
1431  if (costchanged[0] < OldCost)
1432  {
1433  for (i = 0L; (i < ma); i++)
1434  {
1435  for (j = 0L; (j < ma); j++)
1436  alpha[i * ma + j] = u[i * ma + j];
1437  beta[i] = w[i];
1438  a[i] = v[i];
1439  }
1440  *cost = costchanged[0];
1441  }
1442 
1443  free(w);
1444  free(u);
1445  free(da);
1446  free(v);
1447  return(!ERROR);
1448  }
1449 
1450 
1451 
1452 
1453  for (i = 0L; (i < ma); i++)
1454  {
1455  for (j = 0L; (j < ma); j++)
1456  alpha[i * ma + j] = u[i * ma + j];
1457  beta[i] = w[i];
1458  a[i] = v[i];
1459  }
1460  *cost = costchanged[0];
1461 
1462 
1463 #ifndef DBL_MIN
1464 #define DBL_MIN 1e-26
1465 #endif
1466 #ifndef DBL_MAX
1467 #define DBL_MAX 1e+26
1468 #endif
1469 
1470 
1471  if (costchanged[0] < OldCost)
1472  {
1473  if (*lambda > DBL_MIN)
1474  {
1475  *lambda /= LambdaScale;
1476  }
1477  else
1478  {
1479  *IteratingStop = 1;
1480  }
1481  }
1482  else
1483  {
1484  if (*lambda < DBL_MAX)
1485  {
1486  *lambda *= LambdaScale;
1487  }
1488  else
1489  {
1490  *IteratingStop = 1;
1491  }
1492  }
1493  free(w);
1494  free(v);
1495  free(u);
1496  free(da);
1497  return(!ERROR);
1498  } /* End of levenberg_cst */
1499 
1500  /* ------------------------------------------------------------------------- */
1501  /* Main routines */
1502  /* ------------------------------------------------------------------------- */
1503  /* Check the input parameters ---------------------------------------------- */
1504  int cstregistrationCheck(struct cstregistrationStruct *Data)
1505  {
1506 
1507  if (Data == (struct cstregistrationStruct *)NULL)
1508  {
1509  WRITE_ERROR(cstregistrationCheck, "Invalid data pointer.");
1510  return(ERROR);
1511  }
1512 
1513  if ((Data->nx_ReDftVolume <= 0L) || (Data->ny_ReDftVolume <= 0L)
1514  || (Data->nz_ReDftVolume <= 0L))
1515  {
1516  WRITE_ERROR(cstregistrationCheck, " Error - ReDftVolume is missing.");
1517  return(ERROR);
1518  }
1519 
1520  if ((Data->nx_ImDftVolume <= 0L) || (Data->ny_ImDftVolume <= 0L)
1521  || (Data->nz_ImDftVolume <= 0L))
1522  {
1523  WRITE_ERROR(cstregistrationCheck, " Error - ImDftVolume is missing.");
1524  return(ERROR);
1525  }
1526 
1527  if ((Data->nx_ReDftImage <= 0L) || (Data->ny_ReDftImage <= 0L))
1528  {
1529  WRITE_ERROR(cstregistrationCheck, " Error - ReDftImage is missing.");
1530  return(ERROR);
1531  }
1532 
1533  if ((Data->nx_ImDftImage <= 0L) || (Data->ny_ImDftImage <= 0L))
1534  {
1535  WRITE_ERROR(cstregistrationCheck, " Error - ImDftImage is missing.");
1536  return(ERROR);
1537  }
1538 
1539  if ((Data->nx_ReDftVolume != Data->nx_ImDftVolume)
1540  || (Data->ny_ReDftVolume != Data->ny_ImDftVolume)
1541  || (Data->nz_ReDftVolume != Data->nz_ImDftVolume))
1542  {
1543  WRITE_ERROR(cstregistrationCheck,
1544  " Error - ReDftVolume and ImDftVolume should have same dimensions.");
1545  return(ERROR);
1546  }
1547 
1548  if ((Data->nx_ReDftImage != Data->nx_ImDftImage)
1549  || (Data->ny_ReDftImage != Data->ny_ImDftImage))
1550  {
1551  WRITE_ERROR(cstregistrationCheck,
1552  " Error - ReDftImage and ImDftImage should have same dimensions.");
1553  return(ERROR);
1554  }
1555 
1556  if ((Data->nx_Weight <= 0L) || (Data->ny_Weight <= 0L))
1557  {
1558  WRITE_ERROR(cstregistrationCheck, " Error - Weight image is missing.");
1559  return(ERROR);
1560  }
1561 
1562  if ((Data->nx_Weight != Data->nx_ImDftImage)
1563  || (Data->ny_Weight != Data->ny_ImDftImage))
1564  {
1565  WRITE_ERROR(cstregistrationCheck,
1566  " Error - Weight should have same dimensions as ReDftImage and ImDftImage.");
1567  return(ERROR);
1568  }
1569 
1570  if (Data->nx_VoxelSize != 3L)
1571  {
1572  WRITE_ERROR(cstregistrationCheck,
1573  " Error - VoxelSize is a 3-element vector.");
1574  return(ERROR);
1575  }
1576  if (((double) *Data->VoxelSize <= 0.0) || ((double) *(Data->VoxelSize + (std::ptrdiff_t) 1L)
1577  <= 0.0) || ((double) *(Data->VoxelSize + (std::ptrdiff_t) 2L) <= 0.0))
1578  {
1579  WRITE_ERROR(cstregistrationCheck,
1580  " Error - VoxelSize must have all positive elements.");
1581  return(ERROR);
1582  }
1583 
1584  if (Data->nx_PixelSize != 2L)
1585  {
1586  WRITE_ERROR(cstregistrationCheck,
1587  " Error - PixelSize is a 2-element vector.");
1588  return(ERROR);
1589  }
1590  if (((double) *Data->PixelSize <= 0.0) ||
1591  ((double) *(Data->PixelSize + (std::ptrdiff_t) 1L) <= 0.0))
1592  {
1593  WRITE_ERROR(cstregistrationCheck,
1594  " Error - PixelSize must have positive elements.");
1595  return(ERROR);
1596  }
1597 
1598  if (Data->nx_Parameters != 5L)
1599  {
1600  WRITE_ERROR(cstregistrationCheck,
1601  " Error - Rotation is a 5-element vector.");
1602  return(ERROR);
1603  }
1604  if (Data->ScaleLambda <= 0.0)
1605  {
1606  WRITE_ERROR(cstregistrationCheck,
1607  " Error - ScaleLambda must be greater than 0.0.");
1608  return(ERROR);
1609  }
1610  if (Data->LambdaInitial < 0.0)
1611  {
1612  WRITE_ERROR(cstregistrationCheck,
1613  " Error - LambdaInitial must be greater than or equal to 0.0.");
1614  return(ERROR);
1615  }
1616  if (Data->MaxNoIter < 0L)
1617  {
1618  WRITE_ERROR(cstregistrationCheck,
1619  " Error - MaxNoIter must be greater or equal to 0.");
1620  return(ERROR);
1621  }
1622  if (Data->MaxNoFailure < 0L)
1623  {
1624  WRITE_ERROR(cstregistrationCheck,
1625  " Error - MaxNoFailure must be greater or equal to 0.");
1626  return(ERROR);
1627  }
1628  if (Data->SatisfNoSuccess < 0L)
1629  {
1630  WRITE_ERROR(cstregistrationCheck,
1631  " Error - SatisfNoSuccess must be greater or equal to 0.");
1632  return(ERROR);
1633  }
1634  if (Data->SatisfNoSuccess + Data->MaxNoFailure > Data->MaxNoIter)
1635  {
1636  WRITE_ERROR(cstregistrationCheck,
1637  " Error - SatisfNoSuccess + MaxNoFailure must be smaller or equal to MaxNoIter.");
1638  return(ERROR);
1639  }
1640  if ((Data->MakeDesiredProj != 0L) && (Data->MakeDesiredProj != 1L))
1641  {
1642  WRITE_ERROR(cstregistrationCheck,
1643  " Error - MakeDesiredProj should be 0 or 1.");
1644  return(ERROR);
1645  }
1646  if (Data->ToleranceAngle < 0.0)
1647  {
1648  WRITE_ERROR(cstregistrationCheck, " Error - ToleranceAngle should be positive.");
1649  return(ERROR);
1650  }
1651  if (Data->ToleranceShift < 0.0)
1652  {
1653  WRITE_ERROR(cstregistrationCheck, " Error - ToleranceShift should be positive.");
1654  return(ERROR);
1655  }
1656  return(!ERROR);
1657 
1658  }
1659 
1660  /* Manage problem size ----------------------------------------------------- */
1661  int cstregistrationSize(struct cstregistrationStruct *Data)
1662  {
1663 
1664  long MaxIter;
1665 
1666  if (Data == (struct cstregistrationStruct *)NULL)
1667  {
1668  WRITE_ERROR(cstregistrationSize, "Invalid data pointer");
1669  return(ERROR);
1670  }
1671 
1672  MaxIter = Data->MaxNoIter + 1L;
1673 
1674  Data->nx_dftProj = Data->nx_ReDftImage;
1675  Data->ny_dftProj = Data->ny_ReDftImage;
1676  Data->nz_dftProj = 2L;
1677 
1678  Data->nx_OutputParameters = MaxIter;
1679  Data->ny_OutputParameters = 5L;
1680 
1681  Data->nx_Cost = MaxIter;
1682  Data->nx_TimePerIter = MaxIter;
1683  Data->nx_Failures = MaxIter;
1684 
1685  return(!ERROR);
1686 
1687  }
1688 
1689  /* Registration ------------------------------------------------------------ */
1690  int cstregistration(struct cstregistrationStruct *Data)
1691  {
1692  const long OrderOfSpline = 3L;
1693  const auto epsilon = DBL_EPSILON;
1694 
1695  int Status = !ERROR, DoDesProj, IteratingStop, FlagMaxIter;
1696 
1697  long MaxIter, MaxIter1, MaxIter2, iter;
1698  long Mx, My, Nx, Ny, Nz, indx0, indy0, indz0, indx, indy, index;
1699  long SizeIm, MaxNumberOfFailures, SatisfNumberOfSuccesses, nSuccess, nFailure;
1700  double *reDftVolume, *imDftVolume, *pntr_ReInp, *pntr_ImInp, *CoefRe, *CoefIm, *par;
1701  double *pntr_par0, *pntr_par1, *pntr_par2, *pntr_par3, *pntr_par4, *pntr_cost;
1702  double *pntr_time, *pntr_FailureIter, *pntr_RedftCompProj, *pntr_ImdftCompProj;
1703  double LambdaScale, lambda, cost, OldCost, tol_angle, tol_shift;
1704  double OneIterInSeconds, *Parameters, *Gradient, *Hessian;
1705  double *Q1, *Q3, *As, *Ap, *hlp;
1706  double vox_x, vox_y, vox_z, pix_x, pix_y, scx1, scy1, S;
1707  double sum_xx_re, sum_xx_im, dftproj_inp_re, dftproj_inp_im, sc_re, sc_im;
1708  time_t time1, time2, *tp1 = nullptr, *tp2 = nullptr;
1709 
1710  if (Data == (struct cstregistrationStruct *)NULL)
1711  {
1712  WRITE_ERROR(cstregistration, "Invalid data pointer");
1713  return(ERROR);
1714  }
1715 
1716  size_t poolSize = (4 * 16) + (2 * 5) + (1 * 25);
1717  auto pool = std::unique_ptr<double[]>(new double[poolSize]);
1718  double *poolNext = pool.get();
1719  auto getNextFromPool = [&](size_t count) {
1720  auto tmp = poolNext;
1721  poolNext += count;
1722  return tmp;
1723  };
1724 
1725  if (( ! pool) || (nullptr == poolNext)) {
1726  WRITE_ERROR(return_gradhesscost, "ERROR - Not enough memory for memory pool");
1727  return(ERROR);
1728  }
1729 
1730  DoDesProj = (int) Data->MakeDesiredProj;
1731 
1732  reDftVolume = Data->ReDftVolume;
1733  imDftVolume = Data->ImDftVolume;
1734 
1735  Nx = Data->nx_ReDftVolume;
1736  Ny = Data->ny_ReDftVolume;
1737  Nz = Data->nz_ReDftVolume;
1738 
1739  indx0 = -Nx / 2L;
1740  indy0 = -Ny / 2L;
1741  indz0 = -Nz / 2L;
1742 
1743  pntr_ReInp = Data->ReDftImage;
1744  pntr_ImInp = Data->ImDftImage;
1745 
1746  Mx = Data->nx_ReDftImage;
1747  My = Data->ny_ReDftImage;
1748 
1749  SizeIm = Mx * My;
1750 
1751  par = Data->VoxelSize;
1752  vox_x = (double) *par++;
1753  vox_y = (double) *par++;
1754  vox_z = (double) *par;
1755 
1756  par = Data->PixelSize;
1757  pix_x = (double) *par++;
1758  pix_y = (double) *par;
1759 
1760  scx1 = 2.0 * PI / ((double) Mx * pix_x);
1761  scy1 = 2.0 * PI / ((double) My * pix_y);
1762 
1763  S = (double)(Nx * Ny * Nz) / (double)(Mx * My);
1764 
1765  LambdaScale = Data->ScaleLambda;
1766  lambda = Data->LambdaInitial;
1767  cost = 0.0;
1768 
1769  MaxIter = Data->MaxNoIter;
1770  MaxIter1 = MaxIter - 1L;
1771  MaxIter2 = MaxIter + 1L;
1772 
1773  MaxNumberOfFailures = Data->MaxNoFailure;
1774  SatisfNumberOfSuccesses = Data->SatisfNoSuccess;
1775  tol_angle = Data->ToleranceAngle;
1776  tol_shift = Data->ToleranceShift;
1777 
1778  Parameters = getNextFromPool(5);
1779 
1780  par = Data->Parameters;
1781  Parameters[0] = (double)(*par++) * PI / 180.0;
1782  Parameters[1] = (double)(*par++) * PI / 180.0;
1783  Parameters[2] = (double)(*par++) * PI / 180.0;
1784  Parameters[3] = (double)(*par++);
1785  Parameters[4] = (double)(*par);
1786 
1787  AllocateVolumeDouble(&CoefRe, Nx, Ny, Nz, &Status);
1788  if (Status == ERROR)
1789  {
1790  WRITE_ERROR(cstregistration, "ERROR - Not enough memory for CoefRe");
1791  return(ERROR);
1792  }
1793 
1794  AllocateVolumeDouble(&CoefIm, Nx, Ny, Nz, &Status);
1795  if (Status == ERROR)
1796  {
1797  WRITE_ERROR(cstregistration, "ERROR - Not enough memory for CoefIm");
1798  FreeVolumeDouble(&CoefRe);
1799  return(ERROR);
1800  }
1801 
1802  ChangeBasisVolume(reDftVolume, CoefRe, Nx, Ny, Nz, CardinalSpline,
1803  BasicSpline, OrderOfSpline, Periodic, epsilon, &Status);
1804  if (Status == ERROR)
1805  {
1806  WRITE_ERROR(cstregistration, "ERROR");
1807  FreeVolumeDouble(&CoefRe);
1808  FreeVolumeDouble(&CoefIm);
1809  return(ERROR);
1810  }
1811 
1812  ChangeBasisVolume(imDftVolume, CoefIm, Nx, Ny, Nz, CardinalSpline,
1813  BasicSpline, OrderOfSpline, Periodic, epsilon, &Status);
1814  if (Status == ERROR)
1815  {
1816  WRITE_ERROR(cstregistration, "ERROR");
1817  FreeVolumeDouble(&CoefRe);
1818  FreeVolumeDouble(&CoefIm);
1819  return(ERROR);
1820  }
1821 
1822  Gradient = getNextFromPool(5);
1823  Hessian = getNextFromPool(25);
1824  Q1 = getNextFromPool(16);
1825 
1826  if (GetIdentitySquareMatrix(Q1, 4L) == ERROR)
1827  {
1828  WRITE_ERROR(cstregistration, "Error returned by GetIdentitySquareMatrix");
1829  FreeVolumeDouble(&CoefRe);
1830  FreeVolumeDouble(&CoefIm);
1831  return(ERROR);
1832  }
1833 
1834  hlp = Q1;
1835  *hlp = (double) Nx;
1836  hlp += (std::ptrdiff_t)5L;
1837  *hlp = (double) Ny;
1838  hlp += (std::ptrdiff_t)5L;
1839  *hlp = (double) Nz;
1840 
1841  Q3 = getNextFromPool(16);
1842  if (GetIdentitySquareMatrix(Q3, 4L) == ERROR)
1843  {
1844  WRITE_ERROR(cstregistration, "Error returned by GetIdentitySquareMatrix");
1845  FreeVolumeDouble(&CoefRe);
1846  FreeVolumeDouble(&CoefIm);
1847  return(ERROR);
1848  }
1849 
1850  hlp = Q3;
1851  *hlp = 1.0 / (double) Mx;
1852  hlp += (std::ptrdiff_t)5L;
1853  *hlp = 1.0 / (double) My;
1854 
1855 
1856  As = getNextFromPool(16);
1857  Ap = getNextFromPool(16);
1858  if (GetIdentitySquareMatrix(As, 4L) == ERROR)
1859  {
1860  WRITE_ERROR(cstregistration, "Error returned by GetIdentitySquareMatrix");
1861  FreeVolumeDouble(&CoefRe);
1862  FreeVolumeDouble(&CoefIm);
1863  return(ERROR);
1864  }
1865 
1866  hlp = As;
1867  *hlp = vox_x;
1868  hlp += (std::ptrdiff_t)5L;
1869  *hlp = vox_y;
1870  hlp += (std::ptrdiff_t)5L;
1871  *hlp = vox_z;
1872 
1873 
1874  if (GetIdentitySquareMatrix(Ap, 4L) == ERROR)
1875  {
1876  WRITE_ERROR(cstregistration, "Error returned by GetIdentitySquareMatrix");
1877  FreeVolumeDouble(&CoefRe);
1878  FreeVolumeDouble(&CoefIm);
1879  return(ERROR);
1880  }
1881 
1882  hlp = Ap;
1883  *hlp = 1.0 / pix_x;
1884  hlp += (std::ptrdiff_t)5L;
1885  *hlp = 1.0 / pix_y;
1886 
1887  // Normalize by the std. dev. the input dft image
1888  sum_xx_re = 0.0;
1889  sum_xx_im = 0.0;
1890  for (indy = 0L; indy < My; indy++)
1891  {
1892  for (indx = 0L; indx < Mx; indx++)
1893  {
1894  index = indy * Mx + indx;
1895  dftproj_inp_re = (double) pntr_ReInp[index];
1896  dftproj_inp_im = (double) pntr_ImInp[index];
1897  sum_xx_re += dftproj_inp_re * dftproj_inp_re;
1898  sum_xx_im += dftproj_inp_im * dftproj_inp_im;
1899  }
1900  }
1901 
1902  index = (My / 2L) * Mx + (Mx / 2L);
1903  dftproj_inp_re = (double) pntr_ReInp[index];
1904  dftproj_inp_im = (double) pntr_ImInp[index];
1905 
1906  sc_re = 1.0 / sqrt(sum_xx_re + sum_xx_im - dftproj_inp_re * dftproj_inp_re - dftproj_inp_im * dftproj_inp_im);
1907  sc_im = sc_re;
1908 
1909  for (indy = 0L; indy < My; indy++)
1910  {
1911  for (indx = 0L; indx < Mx; indx++)
1912  {
1913  index = indy * Mx + indx;
1914  dftproj_inp_re = (double) pntr_ReInp[index];
1915  dftproj_inp_im = (double) pntr_ImInp[index];
1916  pntr_ReInp[index] = (sc_re * dftproj_inp_re);
1917  pntr_ImInp[index] = (sc_im * dftproj_inp_im);
1918  }
1919  }
1920 
1921  pntr_RedftCompProj = Data->dftProj;
1922  pntr_ImdftCompProj = pntr_RedftCompProj + (std::ptrdiff_t) SizeIm;
1923  for (indy = 0L; indy < My; indy++)
1924  {
1925  for (indx = 0L; indx < Mx; indx++)
1926  {
1927  index = indy * Mx + indx;
1928  pntr_RedftCompProj[index] = 0.0;
1929  pntr_ImdftCompProj[index] = 0.0;
1930  }
1931  }
1932 
1933  pntr_par0 = Data->OutputParameters;
1934  pntr_par1 = pntr_par0 + (std::ptrdiff_t) MaxIter2;
1935  pntr_par2 = pntr_par1 + (std::ptrdiff_t) MaxIter2;
1936  pntr_par3 = pntr_par2 + (std::ptrdiff_t) MaxIter2;
1937  pntr_par4 = pntr_par3 + (std::ptrdiff_t) MaxIter2;
1938  pntr_cost = Data->Cost;
1939  pntr_time = Data->TimePerIter;
1940  pntr_FailureIter = Data->Failures;
1941  for (indx = 0L; indx < MaxIter2; indx++)
1942  {
1943  *pntr_par0++ = 0.0;
1944  *pntr_par1++ = 0.0;
1945  *pntr_par2++ = 0.0;
1946  *pntr_par3++ = 0.0;
1947  *pntr_par4++ = 0.0;
1948  *pntr_cost++ = 0.0;
1949  *pntr_time++ = 0.0;
1950  *pntr_FailureIter++ = 0.0;
1951  }
1952 
1953  time1 = time(tp1);
1954 
1955  if (return_gradhesscost(Gradient, Hessian, &cost, Parameters,
1956  CoefRe, CoefIm, pntr_ReInp, pntr_ImInp, Data->Weight,
1957  Nx, Ny, Nz, indx0, indy0, indz0, Mx, My,
1958  Ap, As, Q1, Q3, scx1, scy1, S, SizeIm,
1959  DoDesProj, Data->dftProj) == ERROR)
1960  {
1961  WRITE_ERROR(cstregistration, "Error returned by return_gradhesscost");
1962  FreeVolumeDouble(&CoefRe);
1963  FreeVolumeDouble(&CoefIm);
1964  return(ERROR);
1965  }
1966 
1967  time2 = time(tp2);
1968  OneIterInSeconds = difftime(time2, time1);
1969 
1970 
1971  if (DoDesProj)
1972  {
1973  FreeVolumeDouble(&CoefRe);
1974  FreeVolumeDouble(&CoefIm);
1975  return(!ERROR);
1976  }
1977 
1978  pntr_par0 = Data->OutputParameters;
1979  pntr_par1 = pntr_par0 + (std::ptrdiff_t) MaxIter2;
1980  pntr_par2 = pntr_par1 + (std::ptrdiff_t) MaxIter2;
1981  pntr_par3 = pntr_par2 + (std::ptrdiff_t) MaxIter2;
1982  pntr_par4 = pntr_par3 + (std::ptrdiff_t) MaxIter2;
1983 
1984  pntr_cost = Data->Cost;
1985  pntr_time = Data->TimePerIter;
1986 
1987  pntr_FailureIter = Data->Failures;
1988 
1989  *pntr_par0++ = Parameters[0];
1990  *pntr_par1++ = Parameters[1];
1991  *pntr_par2++ = Parameters[2];
1992  *pntr_par3++ = Parameters[3];
1993  *pntr_par4++ = Parameters[4];
1994  *pntr_cost++ = cost;
1995  *pntr_time++ = OneIterInSeconds;
1996  pntr_FailureIter++;
1997 
1998  if ((MaxIter == 0L) && (!DoDesProj))
1999  {
2000  FreeVolumeDouble(&CoefRe);
2001  FreeVolumeDouble(&CoefIm);
2002  return(!ERROR);
2003  }
2004 
2005  nSuccess = 0L;
2006  nFailure = 0L;
2007  OldCost = cost;
2008  iter = - 1L;
2009  IteratingStop = 0;
2010  FlagMaxIter = (MaxIter != 1L);
2011  if (!FlagMaxIter)
2012  cost = - 1.0;
2013 
2014  do
2015  {
2016  time1 = time(tp1);
2017 
2018  if (levenberg_cst(Gradient, Hessian, &cost, Parameters,
2019  CoefRe, CoefIm, pntr_ReInp, pntr_ImInp, Data->Weight,
2020  Nx, Ny, Nz, indx0, indy0, indz0, Mx, My,
2021  Ap, As, Q1, Q3, scx1, scy1, S, SizeIm,
2022  DoDesProj, Data->dftProj, OldCost, &lambda, LambdaScale,
2023  &iter, tol_angle, tol_shift, &IteratingStop) == ERROR)
2024  {
2025  WRITE_ERROR(cstregistration, "Error returned by levenberg_cst");
2026  FreeVolumeDouble(&CoefRe);
2027  FreeVolumeDouble(&CoefIm);
2028  return(ERROR);
2029  }
2030 
2031  time2 = time(tp2);
2032  OneIterInSeconds = difftime(time2, time1);
2033 
2034  *pntr_par0++ = Parameters[0];
2035  *pntr_par1++ = Parameters[1];
2036  *pntr_par2++ = Parameters[2];
2037  *pntr_par3++ = Parameters[3];
2038  *pntr_par4++ = Parameters[4];
2039  *pntr_cost++ = cost;
2040  *pntr_time++ = OneIterInSeconds;
2041 
2042  if (cost < OldCost)
2043  {
2044  OldCost = cost;
2045  nSuccess++;
2046  pntr_FailureIter++;
2047  if (nSuccess >= SatisfNumberOfSuccesses)
2048  {
2049  break;
2050  }
2051  if (IteratingStop)
2052  {
2053  break;
2054  }
2055  }
2056  else
2057  {
2058  nFailure++;
2059  *pntr_FailureIter++ = (iter + 1L);
2060  }
2061 
2062  }
2063  while ((nFailure <= MaxNumberOfFailures) && (iter < MaxIter1) && FlagMaxIter);
2064 
2065  *(Data->NumberIterPerformed) = (iter + 1L);
2066  *(Data->NumberSuccPerformed) = nSuccess;
2067  *(Data->NumberFailPerformed) = nFailure;
2068 
2069  FreeVolumeDouble(&CoefRe);
2070  FreeVolumeDouble(&CoefIm);
2071 
2072  return(!ERROR);
2073  }
MDL_ANGLE_ROT
Rotation angle of an image (double,degrees)
Definition: metadata_label.h:55
DBL_EPSILON
#define DBL_EPSILON
YSIZE
#define YSIZE(v)
Definition: multidim_array_base.h:95
return_gradhesscost
int return_gradhesscost(double *Gradient, double *Hessian, double *cost, double *Parameters, double *CoefRe, double *CoefIm, double *pntr_ReInp, double *pntr_ImInp, double *pntr_Weight, long Nx, long Ny, long Nz, long indx0, long indy0, long indz0, long Mx, long My, double *Left, double *Right, double *Q1, double *Q3, double scx1, double scy1, double S, long SizeIm, int DoDesProj, double *dftCompProj)
Definition: angular_continuous_assign.cpp:571
MultidimArray::resize
void resize(size_t Ndim, size_t Zdim, size_t Ydim, size_t Xdim, bool copy=true)
Definition: multidim_array.cpp:983
XmippProgram::getDoubleParam
double getDoubleParam(const char *param, int arg=0)
Definition: xmipp_program.cpp:341
ProgAngularContinuousAssign::processImage
void processImage(const FileName &fnImg, const FileName &fnImgOut, const MDRow &rowIn, MDRow &rowOut)
Definition: angular_continuous_assign.cpp:191
floor
__host__ __device__ float2 floor(const float2 v)
Definition: cuda_basic_math.h:175
cstregistrationCheck
int cstregistrationCheck(struct cstregistrationStruct *Data)
Definition: angular_continuous_assign.cpp:1504
cstregistrationSize
int cstregistrationSize(struct cstregistrationStruct *Data)
Definition: angular_continuous_assign.cpp:1661
Euler_angles2matrix
void Euler_angles2matrix(T alpha, T beta, T gamma, Matrix2D< T > &A, bool homogeneous)
Definition: geometry.cpp:624
periodization
long periodization(long period, long k)
Definition: angular_continuous_assign.cpp:408
c
doublereal * c
Definition: numerical_recipes.cpp:2230
Mask
Definition: mask.h:360
xmipp_funcs.h
alglib::beta
double beta(const double a, const double b)
Definition: specialfunctions.cpp:962
sqrt
void sqrt(Image< double > &op)
Definition: image_operate.cpp:210
MDL_ANGLE_TILT
Tilting angle of an image (double,degrees)
Definition: metadata_label.h:59
y
static double * y
Definition: numerical_recipes.cpp:8487
VectorScalarProduct
int VectorScalarProduct(double A[], double B[], double *X, long Lines)
MDL_SHIFT_X
Shift for the image in the X axis (double)
Definition: metadata_label.h:431
Mask::sigma
double sigma
Definition: mask.h:442
angular_continuous_assign.h
MatrixTimesVector
int MatrixTimesVector(double *A, double *B, double *X, long Lines, long Columns)
MULTIDIM_ARRAY
#define MULTIDIM_ARRAY(v)
Definition: multidim_array_base.h:126
gradhesscost_atpixel
int gradhesscost_atpixel(double *Gradient, double *Hessian, double *cost, double Difference, double dp0, double dp1, double dp2, double dp3, double dp4, double Weight)
Definition: angular_continuous_assign.cpp:451
MDL_ANGLE_PSI
Special label to be used when gathering MDs in MpiMetadataPrograms.
Definition: metadata_label.h:51
w
doublereal * w
Definition: numerical_recipes.cpp:2477
ProgAngularContinuousAssign::defineParams
void defineParams()
Define parameters.
Definition: angular_continuous_assign.cpp:86
iter
glob_prnt iter
Definition: numerical_recipes.cpp:4698
WRITE_ERROR
#define WRITE_ERROR(FunctionName, String)
Definition: error.h:27
CC
#define CC
CC identifier.
Definition: grids.h:585
CSTSplineAssignment
double CSTSplineAssignment(MultidimArray< double > &ReDFTVolume, MultidimArray< double > &ImDFTVolume, MultidimArray< double > &image, MultidimArray< double > &weight, Matrix1D< double > &pose_parameters, int max_no_iter)
Definition: angular_continuous_assign.cpp:256
BSPLINE03DIFF1
#define BSPLINE03DIFF1(y, x)
Definition: kerneldiff1.h:60
x
doublereal * x
Definition: numerical_recipes.cpp:2230
i
#define i
Definition: numerical_recipes.cpp:2493
k
ql0001_ & k(htemp+1),(cvec+1),(atemp+1),(bj+1),(bl+1),(bu+1),(x+1),(clamda+1), &iout, infoqp, &zero,(w+1), &lenw,(iw+1), &leniw, &glob_grd.epsmac
ProgAngularContinuousAssign::imDFTVolume
MultidimArray< double > imDFTVolume
Definition: angular_continuous_assign.h:62
theta
double theta
Definition: numerical_recipes.cpp:6214
CenterFFT
void CenterFFT(MultidimArray< T > &v, bool forward)
Definition: xmipp_fft.h:291
Mask::apply_mask
void apply_mask(const MultidimArray< T > &I, MultidimArray< T > &result, T subs_val=0, bool apply_geo=false)
Definition: mask.h:635
VolumeDftRealToRealImaginary
int VolumeDftRealToRealImaginary(double *Re2Re, double *ImOut, long Nx, long Ny, long Nz, int *Status)
b
doublereal * b
Definition: numerical_recipes.cpp:2230
MDRow::getValue
T & getValue(MDLabel label)
Definition: metadata_row_base.h:97
y0
#define y0
XmippProgram::getParam
const char * getParam(const char *param, int arg=0)
Definition: xmipp_program.cpp:321
lambda
double * lambda
Definition: numerical_recipes.cpp:4414
x0
#define x0
ProgAngularContinuousAssign::readParams
void readParams()
Read argument from command line.
Definition: angular_continuous_assign.cpp:57
index
viol index
Definition: numerical_recipes.cpp:5197
MDL_COST
Cost for the image (double)
Definition: metadata_label.h:90
Euler_distanceBetweenMatrices
double Euler_distanceBetweenMatrices(const Matrix2D< double > &E1, const Matrix2D< double > &E2)
Definition: geometry.cpp:671
PERIODIZATION
#define PERIODIZATION(y, period, k)
Definition: angular_continuous_assign.cpp:427
ERROR
#define ERROR
Definition: configs.h:24
AllocateVolumeDouble
int AllocateVolumeDouble(double **Volume, long Nx, long Ny, long Nz, int *Status)
XSIZE
#define XSIZE(v)
Definition: multidim_array_base.h:91
Mask::type
int type
Definition: mask.h:402
ABS
#define ABS(x)
Definition: xmipp_macros.h:142
free
free((char *) ob)
ZSIZE
#define ZSIZE(v)
Definition: multidim_array_base.h:99
z
double z
Definition: numerical_recipes.cpp:8490
FreeVolumeDouble
int FreeVolumeDouble(double **Volume)
XmippProgram::addExampleLine
void addExampleLine(const char *example, bool verbatim=true)
Definition: xmipp_program.cpp:286
ProgAngularContinuousAssign::reDFTVolume
MultidimArray< double > reDFTVolume
Definition: angular_continuous_assign.h:60
XmippProgram::verbose
int verbose
Verbosity level.
Definition: xmipp_program.h:161
SingularValueBackSubstitution
int SingularValueBackSubstitution(double *U, double W[], double *V, long Lines, long Columns, double B[], double X[], int *Status)
Mask::get_cont_mask
const MultidimArray< double > & get_cont_mask() const
Definition: mask.h:728
Matrix1D::initZeros
void initZeros()
Definition: matrix1d.h:592
MatrixTranspose
int MatrixTranspose(double *A, double *At, long Lines, long Columns)
j
#define j
Definition: numerical_recipes.cpp:2493
m
int m
Definition: image_find_center.cpp:57
DBL_MAX
#define DBL_MAX
DBL_MIN
#define DBL_MIN
levenberg_cst
int levenberg_cst(double beta[], double *alpha, double *cost, double a[], double *CoefRe, double *CoefIm, double *pntr_ReInp, double *pntr_ImInp, double *pntr_Weight, long Nx, long Ny, long Nz, long indx0, long indy0, long indz0, long Mx, long My, double *Left, double *Right, double *Q1, double *Q3, double scx1, double scy1, double S, long SizeIm, int DoDesProj, double *dftCompProj, double OldCost, double *lambda, double LambdaScale, long *iter, double tol_angle, double tol_shift, int *IteratingStop)
Definition: angular_continuous_assign.cpp:1260
MDRow::setValue
void setValue(MDLabel label, const T &d, bool addLabel=true)
Definition: metadata_row_base.h:151
XmippMetadataProgram::show
void show() const override
Definition: xmipp_metadata_program.cpp:232
Mask::generate_mask
void generate_mask(bool apply_geo=false)
Definition: mask.cpp:1577
GAUSSIAN_MASK
#define GAUSSIAN_MASK
Definition: mask.h:369
MATRIX1D_ARRAY
#define MATRIX1D_ARRAY(v)
Definition: matrix1d.h:58
RAD2DEG
#define RAD2DEG(r)
Definition: xmipp_macros.h:320
alglib::psi
double psi(const double x)
Definition: specialfunctions.cpp:2492
SingularValueDecomposition
int SingularValueDecomposition(double *U, long Lines, long Columns, double W[], double *V, long MaxIterations, int *Status)
ProgAngularContinuousAssign::ProgAngularContinuousAssign
ProgAngularContinuousAssign()
Empty constructor.
Definition: angular_continuous_assign.cpp:51
GetIdentitySquareMatrix
int GetIdentitySquareMatrix(double *A, long Size)
mirroring
long mirroring(long period, long k)
Definition: angular_continuous_assign.cpp:391
SVDMAXITER
#define SVDMAXITER
Definition: linearalgebra.h:257
u
doublereal * u
Definition: numerical_recipes.cpp:2230
ChangeBasisVolume
int ChangeBasisVolume(double *VolumeSource, double *VolumeDestination, long Nx, long Ny, long Nz, enum TSplineBasis FromBasis, enum TSplineBasis ToBasis, long Degree, enum TBoundaryConvention Convention, double Tolerance, int *Status)
multiply_3Matrices
int multiply_3Matrices(double *A, double *B, double *C, double *X, long Lines, long CommonSizeH, long CommonSizeW, long Columns)
ImageBase::read
int read(const FileName &name, DataMode datamode=DATA, size_t select_img=ALL_IMAGES, bool mapData=false, int mode=WRITE_READONLY)
Definition: xmipp_image_base.cpp:119
MDL_SHIFT_Y
Shift for the image in the Y axis (double)
Definition: metadata_label.h:435
XmippProgram::addUsageLine
void addUsageLine(const char *line, bool verbatim=false)
Definition: xmipp_program.cpp:282
xmipp_fft.h
PI
#define PI
Definition: tools.h:43
cstregistration
int cstregistration(struct cstregistrationStruct *Data)
Definition: angular_continuous_assign.cpp:1690
XmippProgram::getIntParam
int getIntParam(const char *param, int arg=0)
Definition: xmipp_program.cpp:331
epsilon
double epsilon
Definition: numerical_recipes.cpp:5702
MATRIX2D_ARRAY
#define MATRIX2D_ARRAY(m)
Definition: matrix2d.h:89
Matrix1D::initConstant
void initConstant(T val)
Definition: matrix1d.cpp:83
n
int * n
Definition: numerical_recipes.cpp:2229
XmippMetadataProgram::produces_a_metadata
bool produces_a_metadata
Indicate that the unique final output file is a Metadata.
Definition: xmipp_metadata_program.h:88
a
doublereal * a
Definition: numerical_recipes.cpp:2230
XmippProgram::addParamsLine
void addParamsLine(const String &line)
Definition: xmipp_program.cpp:305
BSPLINE03
#define BSPLINE03(y, x)
Definition: kernel.h:83
Mask::mode
int mode
Definition: mask.h:407
XmippProgram::defaultComments
std::map< String, CommentList > defaultComments
Definition: xmipp_program.h:83
INNER_MASK
constexpr int INNER_MASK
Definition: mask.h:47
Generated by   doxygen 1.8.13