// Show a complex array ---------------------------------------------------
std::ostream& operator<<(std::ostream& ostrm,
const MultidimArray< Complex >& v)
{
if (v.xdim == 0)
ostrm << "NULL MultidimArray\n";
else
ostrm << std::endl;
for (int l = 0; l < NSIZE(v); l++)
{
if (NSIZE(v)>1) ostrm << "Image No. " << l << std::endl;
for (int k = STARTINGZ(v); k <= FINISHINGZ(v); k++)
{
if (ZSIZE(v) > 1) ostrm << "Slice No. " << k << std::endl;
for (int i = STARTINGY(v); i <= FINISHINGY(v); i++)
{
for (int j = STARTINGX(v); j <= FINISHINGX(v); j++)
ostrm << "(" << A3D_ELEM(v, k, i, j).real << "," << A3D_ELEM(v, k, i, j).imag << ")" << ' ';
ostrm << std::endl;
}
}
}
return ostrm;
}
void calculateBackgroundAvgStddev(Image<DOUBLE> &I, DOUBLE &avg, DOUBLE &stddev, int bg_radius)
{
int bg_radius2 = bg_radius * bg_radius;
DOUBLE n = 0.;
avg = 0.;
stddev = 0.;
// Calculate avg in the background pixels
FOR_ALL_ELEMENTS_IN_ARRAY3D(I())
{
if (k*k + i*i + j*j > bg_radius2)
{
avg += A3D_ELEM(I(), k, i, j);
n += 1.;
}
}
avg /= n;
// Calculate stddev in the background pixels
FOR_ALL_ELEMENTS_IN_ARRAY3D(I())
{
if (k*k + i*i + j*j > bg_radius2)
{
DOUBLE aux = A3D_ELEM(I(), k, i, j) - avg;
stddev += aux * aux;
}
}
stddev = sqrt(stddev/n);
}
void Projector::griddingCorrect(MultidimArray<DOUBLE> &vol_in)
{
// Correct real-space map by dividing it by the Fourier transform of the interpolator(s)
vol_in.setXmippOrigin();
FOR_ALL_ELEMENTS_IN_ARRAY3D(vol_in)
{
DOUBLE r = sqrt((DOUBLE)(k*k+i*i+j*j));
// if r==0: do nothing (i.e. divide by 1)
if (r > 0.)
{
DOUBLE rval = r / (ori_size * padding_factor);
DOUBLE sinc = sin(PI * rval) / ( PI * rval);
//DOUBLE ftblob = blob_Fourier_val(rval, blob) / blob_Fourier_val(0., blob);
// Interpolation (goes with "interpolator") to go from arbitrary to fine grid
if (interpolator==NEAREST_NEIGHBOUR && r_min_nn == 0)
{
// NN interpolation is convolution with a rectangular pulse, which FT is a sinc function
A3D_ELEM(vol_in, k, i, j) /= sinc;
}
else if (interpolator==TRILINEAR || (interpolator==NEAREST_NEIGHBOUR && r_min_nn > 0) )
{
// trilinear interpolation is convolution with a triangular pulse, which FT is a sinc^2 function
A3D_ELEM(vol_in, k, i, j) /= sinc * sinc;
}
else
REPORT_ERROR("BUG Projector::griddingCorrect: unrecognised interpolator scheme.");
//#define DEBUG_GRIDDING_CORRECT
#ifdef DEBUG_GRIDDING_CORRECT
if (k==0 && i==0 && j > 0)
std::cerr << " j= " << j << " sinc= " << sinc << std::endl;
#endif
}
}
}
double ProgResolutionIBW::calculateIBW(MultidimArray <double>& widths) const
{
double total, count;
total=count=0;
FOR_ALL_ELEMENTS_IN_ARRAY3D(widths)
{
double width=A3D_ELEM(widths,k,i,j);
if (width<1e4)
{
total+=width;
count++;
}
}
double avg = total/count;
return 1.0/avg;
}
void * blobs2voxels_SimpleGrid( void * data )
{
ThreadBlobsToVoxels * thread_data = (ThreadBlobsToVoxels *) data;
const MultidimArray<double> *vol_blobs = thread_data->vol_blobs;
const SimpleGrid *grid = thread_data->grid;
const struct blobtype *blob = thread_data->blob;
MultidimArray<double> *vol_voxels = thread_data->vol_voxels;
const Matrix2D<double> *D = thread_data->D;
int istep = thread_data->istep;
MultidimArray<double> *vol_corr = thread_data->vol_corr;
const MultidimArray<double> *vol_mask = thread_data->vol_mask;
;
bool FORW = thread_data->FORW;
int eq_mode = thread_data->eq_mode;
int min_separation = thread_data->min_separation;
int z_planes = (int)(ZZ(grid->highest) - ZZ(grid->lowest) + 1);
Matrix2D<double> Dinv; // Inverse of D
Matrix1D<double> act_coord(3); // Coord: Actual position inside
// the voxel volume without deforming
Matrix1D<double> real_position(3); // Coord: actual position after
// applying the V transformation
Matrix1D<double> beginZ(3); // Coord: Voxel coordinates of the
// blob at the 3D point
// (z0,YY(lowest),XX(lowest))
Matrix1D<double> beginY(3); // Coord: Voxel coordinates of the
// blob at the 3D point
// (z0,y0,XX(lowest))
Matrix1D<double> corner2(3), corner1(3); // Coord: Corners of the
// blob in the voxel volume
Matrix1D<double> gcurrent(3); // Position in g of current point
MultidimArray<double> blob_table; // Something like a blobprint
// but with the values of the
// blob in space
double d; // Distance between the center
// of the blob and a voxel position
int id; // index inside the blob value
// table for tha blob value at
// a distance d
double intx, inty, intz; // Nearest integer voxel
int i, j, k; // Index within the blob volume
int process; // True if this blob has to be
// processed
double vol_correction=0; // Correction to apply to the
// volume when "projecting" back
SPEED_UP_temps012;
// Some aliases
#define x0 STARTINGX(*vol_voxels)
#define xF FINISHINGX(*vol_voxels)
#define y0 STARTINGY(*vol_voxels)
#define yF FINISHINGY(*vol_voxels)
#define z0 STARTINGZ(*vol_voxels)
#define zF FINISHINGZ(*vol_voxels)
#ifdef DEBUG
bool condition = !FORW;
if (condition)
{
(*vol_voxels)().printShape();
std::cout << std::endl;
std::cout << "x0= " << x0 << " xF= " << xF << std::endl;
std::cout << "y0= " << y0 << " yF= " << yF << std::endl;
std::cout << "z0= " << z0 << " zF= " << zF << std::endl;
std::cout << grid;
}
#endif
// Invert deformation matrix ............................................
if (D != NULL)
Dinv = D->inv();
// Compute a blob value table ...........................................
blob_table.resize((int)(blob->radius*istep + 1));
for (size_t i = 0; i < blob_table.xdim; i++)
{
A1D_ELEM(blob_table, i) = kaiser_value((double)i/istep, blob->radius, blob->alpha, blob->order);
#ifdef DEBUG_MORE
if (condition)
std::cout << "Blob (" << i << ") r=" << (double)i / istep <<
" val= " << A1D_ELEM(blob_table, i) << std::endl;
#endif
}
int assigned_slice;
do
{
assigned_slice = -1;
do
{
pthread_mutex_lock(&blobs_conv_mutex);
if( slices_processed == z_planes )
{
pthread_mutex_unlock(&blobs_conv_mutex);
return (void*)NULL;
}
for(int w = 0 ; w < z_planes ; w++ )
{
if( slices_status[w]==0 )
{
slices_status[w] = -1;
assigned_slice = w;
slices_processed++;
for( int in = (w-min_separation+1) ; in <= (w+min_separation-1 ) ; in ++ )
{
if( in != w )
{
if( ( in >= 0 ) && ( in < z_planes ))
{
if( slices_status[in] != -1 )
slices_status[in]++;
}
}
}
break;
}
}
pthread_mutex_unlock(&blobs_conv_mutex);
}
while( assigned_slice == -1);
// Convert the whole grid ...............................................
// Corner of the plane defined by Z. These coordinates are in the
// universal coord. system
Matrix1D<double> aux( grid->lowest );
k = (int)(assigned_slice + ZZ( grid->lowest ));
ZZ(aux) = k;
grid->grid2universe(aux, beginZ);
Matrix1D<double> grid_index(3);
// Corner of the row defined by Y
beginY = beginZ;
for (i = (int) YY(grid->lowest); i <= (int) YY(grid->highest); i++)
{
// First point in the row
act_coord = beginY;
for (j = (int) XX(grid->lowest); j <= (int) XX(grid->highest); j++)
{
VECTOR_R3(grid_index, j, i, k);
#ifdef DEBUG
if (condition)
{
printf("Dealing blob at (%d,%d,%d) = %f\n", j, i, k, A3D_ELEM(*vol_blobs, k, i, j));
std::cout << "Center of the blob "
<< act_coord.transpose() << std::endl;
}
#endif
// Place act_coord in its right place
if (D != NULL)
{
M3x3_BY_V3x1(real_position, *D, act_coord);
#ifdef DEBUG
if (condition)
std::cout << "Center of the blob moved to "
//ROB, the "moved" coordinates are in
// real_position not in act_coord
<< act_coord.transpose() << std::endl;
<< real_position.transpose() << std::endl;
#endif
// ROB This is OK if blob.radius is in Cartesian space as I
// think is the case
}
else
real_position = act_coord;
// These two corners are also real valued
process = true;
//ROB
//This is OK if blob.radius is in Cartesian space as I think is the case
V3_PLUS_CT(corner1, real_position, -blob->radius);
V3_PLUS_CT(corner2, real_position, blob->radius);
#ifdef DEFORM_BLOB_WHEN_IN_CRYSTAL
//ROB
//we do not need this, it is already in Cartesian space
//if (D!=NULL)
// box_enclosing(corner1,corner2, *D, corner1, corner2);
#endif
if (XX(corner1) >= xF)
process = false;
if (YY(corner1) >= yF)
process = false;
if (ZZ(corner1) >= zF)
process = false;
if (XX(corner2) <= x0)
process = false;
if (YY(corner2) <= y0)
process = false;
if (ZZ(corner2) <= z0)
process = false;
#ifdef DEBUG
if (!process && condition)
std::cout << " It is outside output volume\n";
#endif
if (!grid->is_interesting(real_position))
{
#ifdef DEBUG
if (process && condition)
std::cout << " It is not interesting\n";
#endif
process = false;
}
#ifdef DEBUG
if (condition)
{
std::cout << "Corner 1 for this point " << corner1.transpose() << std::endl;
std::cout << "Corner 2 for this point " << corner2.transpose() << std::endl;
}
#endif
if (process)
{
// Clip the corners to the volume borders
XX(corner1) = ROUND(CLIP(XX(corner1), x0, xF));
YY(corner1) = ROUND(CLIP(YY(corner1), y0, yF));
ZZ(corner1) = ROUND(CLIP(ZZ(corner1), z0, zF));
XX(corner2) = ROUND(CLIP(XX(corner2), x0, xF));
YY(corner2) = ROUND(CLIP(YY(corner2), y0, yF));
ZZ(corner2) = ROUND(CLIP(ZZ(corner2), z0, zF));
#ifdef DEBUG
if (condition)
{
std::cout << "Clipped and rounded Corner 1 " << corner1.transpose() << std::endl;
std::cout << "Clipped and rounded Corner 2 " << corner2.transpose() << std::endl;
}
#endif
if (!FORW)
switch (eq_mode)
{
case VARTK:
vol_correction = 0;
break;
case VMAXARTK:
vol_correction = -1e38;
break;
}
// Effectively convert
long N_eq;
N_eq = 0;
for (intz = ZZ(corner1); intz <= ZZ(corner2); intz++)
for (inty = YY(corner1); inty <= YY(corner2); inty++)
for (intx = XX(corner1); intx <= XX(corner2); intx++)
{
int iz = (int)intz, iy = (int)inty, ix = (int)intx;
if (vol_mask != NULL)
if (!A3D_ELEM(*vol_mask, iz, iy, ix))
continue;
// Compute distance to the center of the blob
VECTOR_R3(gcurrent, intx, inty, intz);
#ifdef DEFORM_BLOB_WHEN_IN_CRYSTAL
// ROB
//if (D!=NULL)
// M3x3_BY_V3x1(gcurrent,Dinv,gcurrent);
#endif
V3_MINUS_V3(gcurrent, real_position, gcurrent);
d = sqrt(XX(gcurrent) * XX(gcurrent) +
YY(gcurrent) * YY(gcurrent) +
ZZ(gcurrent) * ZZ(gcurrent));
if (d > blob->radius)
continue;
id = (int)(d * istep);
#ifdef DEBUG_MORE
if (condition)
{
std::cout << "At (" << intx << ","
<< inty << "," << intz << ") distance=" << d;
std::cout.flush();
}
#endif
// Add at that position the corresponding blob value
if (FORW)
{
A3D_ELEM(*vol_voxels, iz, iy, ix) +=
A3D_ELEM(*vol_blobs, k, i, j) *
A1D_ELEM(blob_table, id);
#ifdef DEBUG_MORE
if (condition)
{
std::cout << " adding " << A3D_ELEM(*vol_blobs, k, i, j)
<< " * " << A1D_ELEM(blob_table, id) << " = "
<< A3D_ELEM(*vol_blobs, k, i, j)*
A1D_ELEM(blob_table, id) << std::endl;
std::cout.flush();
}
#endif
if (vol_corr != NULL)
A3D_ELEM(*vol_corr, iz, iy, ix) +=
A1D_ELEM(blob_table, id) * A1D_ELEM(blob_table, id);
}
else
{
double contrib = A3D_ELEM(*vol_corr, iz, iy, ix) *
A1D_ELEM(blob_table, id);
switch (eq_mode)
{
case VARTK:
vol_correction += contrib;
N_eq++;
break;
case VMAXARTK:
if (contrib > vol_correction)
vol_correction = contrib;
break;
}
#ifdef DEBUG_MORE
if (condition)
{
std::cout << " adding " << A3D_ELEM(*vol_corr, iz, iy, ix)
<< " * " << A1D_ELEM(blob_table, id) << " = "
<< contrib << std::endl;
std::cout.flush();
}
#endif
}
}
if (N_eq == 0)
N_eq = 1;
if (!FORW)
{
A3D_ELEM(*vol_blobs, k, i, j) += vol_correction / N_eq;
#ifdef DEBUG_MORE
std::cout << " correction= " << vol_correction << std::endl
<< " Number of eqs= " << N_eq << std::endl
<< " Blob after correction= "
<< A3D_ELEM(*vol_blobs, k, i, j) << std::endl;
#endif
}
}
// Prepare for next iteration
XX(act_coord) = XX(act_coord) + grid->relative_size * (grid->basis)( 0, 0);
YY(act_coord) = YY(act_coord) + grid->relative_size * (grid->basis)( 1, 0);
ZZ(act_coord) = ZZ(act_coord) + grid->relative_size * (grid->basis)( 2, 0);
}
//#define DEBUG
void spatial_Bspline032voxels(const GridVolume &vol_splines,
MultidimArray<double> *vol_voxels, int Zdim, int Ydim, int Xdim)
{
// Resize and set starting corner .......................................
if (Zdim == 0 || Ydim == 0 || Xdim == 0)
{
Matrix1D<double> size = vol_splines.grid(0).highest -
vol_splines.grid(0).lowest;
Matrix1D<double> corner = vol_splines.grid(0).lowest;
(*vol_voxels).initZeros(CEIL(ZZ(size)), CEIL(YY(size)), CEIL(XX(size)));
STARTINGX(*vol_voxels) = FLOOR(XX(corner));
STARTINGY(*vol_voxels) = FLOOR(YY(corner));
STARTINGZ(*vol_voxels) = FLOOR(ZZ(corner));
}
else
{
(*vol_voxels).initZeros(Zdim, Ydim, Xdim);
(*vol_voxels).setXmippOrigin();
}
// Convert each subvolume ...............................................
for (size_t i = 0; i < vol_splines.VolumesNo(); i++)
{
spatial_Bspline032voxels_SimpleGrid(vol_splines(i)(), vol_splines.grid(i),
vol_voxels);
#ifdef DEBUG
std::cout << "Spline grid no " << i << " stats: ";
vol_splines(i)().printStats();
std::cout << std::endl;
std::cout << "So far vol stats: ";
(*vol_voxels).printStats();
std::cout << std::endl;
VolumeXmipp save;
save() = *vol_voxels;
save.write((std::string)"PPPvoxels" + integerToString(i));
#endif
}
// Now normalise the resulting volume ..................................
double inorm = 1.0 / sum_spatial_Bspline03_Grid(vol_splines.grid());
FOR_ALL_ELEMENTS_IN_ARRAY3D(*vol_voxels)
A3D_ELEM(*vol_voxels, k, i, j) *= inorm;
// Set voxels outside interest region to minimum value .................
double R = vol_splines.grid(0).get_interest_radius();
if (R != -1)
{
double R2 = (R - 6) * (R - 6);
// Compute minimum value within sphere
double min_val = A3D_ELEM(*vol_voxels, 0, 0, 0);
FOR_ALL_ELEMENTS_IN_ARRAY3D(*vol_voxels)
if (j*j + i*i + k*k <= R2 - 4)
min_val = XMIPP_MIN(min_val, A3D_ELEM(*vol_voxels, k, i, j));
// Substitute minimum value
R2 = (R - 2) * (R - 2);
FOR_ALL_ELEMENTS_IN_ARRAY3D(*vol_voxels)
if (j*j + i*i + k*k >= R2)
A3D_ELEM(*vol_voxels, k, i, j) = min_val;
}
/* Splines -> Voxels for a SimpleGrid -------------------------------------- */
void spatial_Bspline032voxels_SimpleGrid(const MultidimArray<double> &vol_splines,
const SimpleGrid &grid,
MultidimArray<double> *vol_voxels,
const MultidimArray<double> *vol_mask = NULL)
{
Matrix1D<double> act_coord(3); // Coord: Actual position inside
// the voxel volume without deforming
Matrix1D<double> beginZ(3); // Coord: Voxel coordinates of the
// blob at the 3D point
// (z0,YY(lowest),XX(lowest))
Matrix1D<double> beginY(3); // Coord: Voxel coordinates of the
// blob at the 3D point
// (z0,y0,XX(lowest))
Matrix1D<double> corner2(3), corner1(3); // Coord: Corners of the
// blob in the voxel volume
Matrix1D<double> gcurrent(3); // Position in g of current point
double intx, inty, intz; // Nearest integer voxel
int i, j, k; // Index within the blob volume
bool process; // True if this blob has to be
// processed
double spline_radius = 2 * sqrt(3.0);
// Some aliases
#define x0 STARTINGX(*vol_voxels)
#define xF FINISHINGX(*vol_voxels)
#define y0 STARTINGY(*vol_voxels)
#define yF FINISHINGY(*vol_voxels)
#define z0 STARTINGZ(*vol_voxels)
#define zF FINISHINGZ(*vol_voxels)
#ifdef DEBUG
bool condition = true;
(*vol_voxels)().printShape();
std::cout << std::endl;
std::cout << "x0= " << x0 << " xF= " << xF << std::endl;
std::cout << "y0= " << y0 << " yF= " << yF << std::endl;
std::cout << "z0= " << z0 << " zF= " << zF << std::endl;
std::cout << grid;
#endif
// Convert the whole grid ...............................................
// Corner of the plane defined by Z. These coordinates are in the
// universal coord. system
grid.grid2universe(grid.lowest, beginZ);
Matrix1D<double> grid_index(3);
for (k = (int) ZZ(grid.lowest); k <= (int) ZZ(grid.highest); k++)
{
// Corner of the row defined by Y
beginY = beginZ;
for (i = (int) YY(grid.lowest); i <= (int) YY(grid.highest); i++)
{
// First point in the row
act_coord = beginY;
for (j = (int) XX(grid.lowest); j <= (int) XX(grid.highest); j++)
{
VECTOR_R3(grid_index, j, i, k);
#ifdef DEBUG
if (condition)
{
printf("Dealing spline at (%d,%d,%d) = %f\n", j, i, k, A3D_ELEM(vol_splines, k, i, j));
std::cout << "Center of the blob "
<< act_coord.transpose() << std::endl;
}
#endif
// These two corners are also real valued
process = true;
if (XX(act_coord) >= xF) process = false;
if (YY(act_coord) >= yF) process = false;
if (ZZ(act_coord) >= zF) process = false;
if (XX(act_coord) <= x0) process = false;
if (YY(act_coord) <= y0) process = false;
if (ZZ(act_coord) <= z0) process = false;
#ifdef DEBUG
if (!process && condition) std::cout << " It is outside output volume\n";
#endif
if (!grid.is_interesting(act_coord))
{
#ifdef DEBUG
if (process && condition) std::cout << " It is not interesting\n";
#endif
process = false;
}
if (process)
{
V3_PLUS_CT(corner1, act_coord, -spline_radius);
V3_PLUS_CT(corner2, act_coord, spline_radius);
#ifdef DEBUG
if (condition)
{
std::cout << "Corner 1 for this point " << corner1.transpose() << std::endl;
std::cout << "Corner 2 for this point " << corner2.transpose() << std::endl;
}
#endif
// Clip the corners to the volume borders
XX(corner1) = ROUND(CLIP(XX(corner1), x0, xF));
YY(corner1) = ROUND(CLIP(YY(corner1), y0, yF));
ZZ(corner1) = ROUND(CLIP(ZZ(corner1), z0, zF));
XX(corner2) = ROUND(CLIP(XX(corner2), x0, xF));
YY(corner2) = ROUND(CLIP(YY(corner2), y0, yF));
ZZ(corner2) = ROUND(CLIP(ZZ(corner2), z0, zF));
#ifdef DEBUG
if (condition)
{
std::cout << "Clipped and rounded Corner 1 " << corner1.transpose() << std::endl;
std::cout << "Clipped and rounded Corner 2 " << corner2.transpose() << std::endl;
}
#endif
// Effectively convert
for (intz = ZZ(corner1); intz <= ZZ(corner2); intz++)
for (inty = YY(corner1); inty <= YY(corner2); inty++)
for (intx = XX(corner1); intx <= XX(corner2); intx++)
{
int iz = (int)intz, iy = (int)inty, ix = (int)intx;
if (vol_mask != NULL)
if (!A3D_ELEM(*vol_mask, iz, iy, ix)) continue;
// Compute the spline value at this point
VECTOR_R3(gcurrent, intx, inty, intz);
V3_MINUS_V3(gcurrent, act_coord, gcurrent);
double spline_value = spatial_Bspline03(gcurrent);
#ifdef DEBUG_MORE
if (condition)
{
std::cout << "At (" << intx << ","
<< inty << "," << intz << ") value="
<< spline_value;
std::cout.flush();
}
#endif
// Add at that position the corresponding spline value
A3D_ELEM(*vol_voxels, iz, iy, ix) +=
A3D_ELEM(vol_splines, k, i, j) * spline_value;
#ifdef DEBUG_MORE
if (condition)
{
std::cout << " adding " << A3D_ELEM(vol_splines, k, i, j)
<< " * " << value_spline << " = "
<< A3D_ELEM(vol_splines, k, i, j)*
value_spline << std::endl;
std::cout.flush();
}
#endif
}
}
// Prepare for next iteration
XX(act_coord) = XX(act_coord) + grid.relative_size * grid.basis(0, 0);
YY(act_coord) = YY(act_coord) + grid.relative_size * grid.basis(1, 0);
ZZ(act_coord) = ZZ(act_coord) + grid.relative_size * grid.basis(2, 0);
}
XX(beginY) = XX(beginY) + grid.relative_size * grid.basis(0, 1);
YY(beginY) = YY(beginY) + grid.relative_size * grid.basis(1, 1);
ZZ(beginY) = ZZ(beginY) + grid.relative_size * grid.basis(2, 1);
}
XX(beginZ) = XX(beginZ) + grid.relative_size * grid.basis(0, 2);
YY(beginZ) = YY(beginZ) + grid.relative_size * grid.basis(1, 2);
ZZ(beginZ) = ZZ(beginZ) + grid.relative_size * grid.basis(2, 2);
}
}
void FourierProjector::project(double rot, double tilt, double psi)
{
double freqy, freqx;
std::complex< double > f;
Euler_angles2matrix(rot,tilt,psi,E);
projectionFourier.initZeros();
double shift=-FIRST_XMIPP_INDEX(volumeSize);
double xxshift = -2 * PI * shift / volumeSize;
double maxFreq2=maxFrequency*maxFrequency;
double volumePaddedSize=XSIZE(VfourierRealCoefs);
for (size_t i=0; i<YSIZE(projectionFourier); ++i)
{
FFT_IDX2DIGFREQ(i,volumeSize,freqy);
double freqy2=freqy*freqy;
double phasey=(double)(i) * xxshift;
double freqYvol_X=MAT_ELEM(E,1,0)*freqy;
double freqYvol_Y=MAT_ELEM(E,1,1)*freqy;
double freqYvol_Z=MAT_ELEM(E,1,2)*freqy;
for (size_t j=0; j<XSIZE(projectionFourier); ++j)
{
// The frequency of pairs (i,j) in 2D
FFT_IDX2DIGFREQ(j,volumeSize,freqx);
// Do not consider pixels with high frequency
if ((freqy2+freqx*freqx)>maxFreq2)
continue;
// Compute corresponding frequency in the volume
double freqvol_X=freqYvol_X+MAT_ELEM(E,0,0)*freqx;
double freqvol_Y=freqYvol_Y+MAT_ELEM(E,0,1)*freqx;
double freqvol_Z=freqYvol_Z+MAT_ELEM(E,0,2)*freqx;
double c,d;
if (BSplineDeg==0)
{
// 0 order interpolation
// Compute corresponding index in the volume
int kVolume=(int)round(freqvol_Z*volumePaddedSize);
int iVolume=(int)round(freqvol_Y*volumePaddedSize);
int jVolume=(int)round(freqvol_X*volumePaddedSize);
c = A3D_ELEM(VfourierRealCoefs,kVolume,iVolume,jVolume);
d = A3D_ELEM(VfourierImagCoefs,kVolume,iVolume,jVolume);
}
else if (BSplineDeg==1)
{
// B-spline linear interpolation
double kVolume=freqvol_Z*volumePaddedSize;
double iVolume=freqvol_Y*volumePaddedSize;
double jVolume=freqvol_X*volumePaddedSize;
c=VfourierRealCoefs.interpolatedElement3D(jVolume,iVolume,kVolume);
d=VfourierImagCoefs.interpolatedElement3D(jVolume,iVolume,kVolume);
}
else
{
// B-spline cubic interpolation
double kVolume=freqvol_Z*volumePaddedSize;
double iVolume=freqvol_Y*volumePaddedSize;
double jVolume=freqvol_X*volumePaddedSize;
c=VfourierRealCoefs.interpolatedElementBSpline3D(jVolume,iVolume,kVolume);
d=VfourierImagCoefs.interpolatedElementBSpline3D(jVolume,iVolume,kVolume);
}
// Phase shift to move the origin of the image to the corner
double dotp = (double)(j) * xxshift + phasey;
double a,b;
sincos(dotp,&b,&a);
// Multiply Fourier coefficient in volume times phase shift
double ac = a * c;
double bd = b * d;
double ab_cd = (a + b) * (c + d);
// And store the multiplication
double *ptrI_ij=(double *)&DIRECT_A2D_ELEM(projectionFourier,i,j);
*ptrI_ij = ac - bd;
*(ptrI_ij+1) = ab_cd - ac - bd;
}
}
//VfourierRealCoefs.clear();
//VfourierImagCoefs.clear();
transformer2D.inverseFourierTransform();
}
// Fill data array with oversampled Fourier transform, and calculate its power spectrum
void Projector::computeFourierTransformMap(MultidimArray<DOUBLE> &vol_in, MultidimArray<DOUBLE> &power_spectrum, int current_size, int nr_threads, bool do_gridding)
{
MultidimArray<DOUBLE> Mpad;
MultidimArray<Complex > Faux;
FourierTransformer transformer;
// DEBUGGING: multi-threaded FFTWs are giving me a headache?
// For a long while: switch them off!
//transformer.setThreadsNumber(nr_threads);
DOUBLE normfft;
// Size of padded real-space volume
int padoridim = padding_factor * ori_size;
// Initialize data array of the oversampled transform
ref_dim = vol_in.getDim();
// Make Mpad
switch (ref_dim)
{
case 2:
Mpad.initZeros(padoridim, padoridim);
normfft = (DOUBLE)(padding_factor * padding_factor);
break;
case 3:
Mpad.initZeros(padoridim, padoridim, padoridim);
if (data_dim ==3)
normfft = (DOUBLE)(padding_factor * padding_factor * padding_factor);
else
normfft = (DOUBLE)(padding_factor * padding_factor * padding_factor * ori_size);
break;
default:
REPORT_ERROR("Projector::computeFourierTransformMap%%ERROR: Dimension of the data array should be 2 or 3");
}
// First do a gridding pre-correction on the real-space map:
// Divide by the inverse Fourier transform of the interpolator in Fourier-space
// 10feb11: at least in 2D case, this seems to be the wrong thing to do!!!
// TODO: check what is best for subtomo!
if (do_gridding)// && data_dim != 3)
griddingCorrect(vol_in);
// Pad translated map with zeros
vol_in.setXmippOrigin();
Mpad.setXmippOrigin();
FOR_ALL_ELEMENTS_IN_ARRAY3D(vol_in) // This will also work for 2D
A3D_ELEM(Mpad, k, i, j) = A3D_ELEM(vol_in, k, i, j);
// Translate padded map to put origin of FT in the center
CenterFFT(Mpad, true);
// Calculate the oversampled Fourier transform
transformer.FourierTransform(Mpad, Faux, false);
// Free memory: Mpad no longer needed
Mpad.clear();
// Resize data array to the right size and initialise to zero
initZeros(current_size);
// Fill data only for those points with distance to origin less than max_r
// (other points will be zero because of initZeros() call above
// Also calculate radial power spectrum
power_spectrum.initZeros(ori_size / 2 + 1);
MultidimArray<DOUBLE> counter(power_spectrum);
counter.initZeros();
int max_r2 = r_max * r_max * padding_factor * padding_factor;
FOR_ALL_ELEMENTS_IN_FFTW_TRANSFORM(Faux) // This will also work for 2D
{
int r2 = kp*kp + ip*ip + jp*jp;
// The Fourier Transforms are all "normalised" for 2D transforms of size = ori_size x ori_size
if (r2 <= max_r2)
{
// Set data array
A3D_ELEM(data, kp, ip, jp) = DIRECT_A3D_ELEM(Faux, k, i, j) * normfft;
// Calculate power spectrum
int ires = ROUND( sqrt((DOUBLE)r2) / padding_factor );
// Factor two because of two-dimensionality of the complex plane
DIRECT_A1D_ELEM(power_spectrum, ires) += norm(A3D_ELEM(data, kp, ip, jp)) / 2.;
DIRECT_A1D_ELEM(counter, ires) += 1.;
}
}
// Calculate radial average of power spectrum
FOR_ALL_DIRECT_ELEMENTS_IN_ARRAY1D(power_spectrum)
{
if (DIRECT_A1D_ELEM(counter, i) < 1.)
DIRECT_A1D_ELEM(power_spectrum, i) = 0.;
else
DIRECT_A1D_ELEM(power_spectrum, i) /= DIRECT_A1D_ELEM(counter, i);
}
transformer.cleanup();
}
void Projector::rotate3D(MultidimArray<Complex > &f3d, Matrix2D<DOUBLE> &A, bool inv)
{
DOUBLE fx, fy, fz, xp, yp, zp;
int x0, x1, y0, y1, z0, z1, y, z, y2, z2, r2;
bool is_neg_x;
Complex d000, d010, d100, d110, d001, d011, d101, d111, dx00, dx10, dxy0, dx01, dx11, dxy1;
Matrix2D<DOUBLE> Ainv;
// f3d should already be in the right size (ori_size,orihalfdim)
// AND the points outside max_r should already be zero...
// f3d.initZeros();
// Use the inverse matrix
if (inv)
Ainv = A;
else
Ainv = A.transpose();
// The f3d image may be smaller than r_max, in that case also make sure not to fill the corners!
int my_r_max = XMIPP_MIN(r_max, XSIZE(f3d) - 1);
// Go from the 3D rotated coordinates to the original map coordinates
Ainv *= (DOUBLE)padding_factor; // take scaling into account directly
int max_r2 = my_r_max * my_r_max;
int min_r2_nn = r_min_nn * r_min_nn;
#ifdef DEBUG
std::cerr << " XSIZE(f3d)= "<< XSIZE(f3d) << std::endl;
std::cerr << " YSIZE(f3d)= "<< YSIZE(f3d) << std::endl;
std::cerr << " XSIZE(data)= "<< XSIZE(data) << std::endl;
std::cerr << " YSIZE(data)= "<< YSIZE(data) << std::endl;
std::cerr << " STARTINGX(data)= "<< STARTINGX(data) << std::endl;
std::cerr << " STARTINGY(data)= "<< STARTINGY(data) << std::endl;
std::cerr << " STARTINGZ(data)= "<< STARTINGZ(data) << std::endl;
std::cerr << " max_r= "<< r_max << std::endl;
std::cerr << " Ainv= " << Ainv << std::endl;
#endif
for (int k=0; k < ZSIZE(f3d); k++)
{
// Don't search beyond square with side max_r
if (k <= my_r_max)
{
z = k;
}
else if (k >= ZSIZE(f3d) - my_r_max)
{
z = k - ZSIZE(f3d);
}
else
continue;
z2 = z * z;
for (int i=0; i < YSIZE(f3d); i++)
{
// Don't search beyond square with side max_r
if (i <= my_r_max)
{
y = i;
}
else if (i >= YSIZE(f3d) - my_r_max)
{
y = i - YSIZE(f3d);
}
else
continue;
y2 = y * y;
for (int x=0; x <= my_r_max; x++)
{
// Only include points with radius < max_r (exclude points outside circle in square)
r2 = x * x + y2 + z2;
if (r2 > max_r2)
continue;
// Get logical coordinates in the 3D map
xp = Ainv(0,0) * x + Ainv(0,1) * y + Ainv(0,2) * z;
yp = Ainv(1,0) * x + Ainv(1,1) * y + Ainv(1,2) * z;
zp = Ainv(2,0) * x + Ainv(2,1) * y + Ainv(2,2) * z;
if (interpolator == TRILINEAR || r2 < min_r2_nn)
{
// Only asymmetric half is stored
if (xp < 0)
{
// Get complex conjugated hermitian symmetry pair
xp = -xp;
yp = -yp;
zp = -zp;
is_neg_x = true;
}
else
{
is_neg_x = false;
}
// Trilinear interpolation (with physical coords)
// Subtract STARTINGY to accelerate access to data (STARTINGX=0)
// In that way use DIRECT_A3D_ELEM, rather than A3D_ELEM
x0 = FLOOR(xp);
fx = xp - x0;
x1 = x0 + 1;
y0 = FLOOR(yp);
fy = yp - y0;
y0 -= STARTINGY(data);
y1 = y0 + 1;
z0 = FLOOR(zp);
fz = zp - z0;
z0 -= STARTINGZ(data);
z1 = z0 + 1;
// Matrix access can be accelerated through pre-calculation of z0*xydim etc.
d000 = DIRECT_A3D_ELEM(data, z0, y0, x0);
d001 = DIRECT_A3D_ELEM(data, z0, y0, x1);
d010 = DIRECT_A3D_ELEM(data, z0, y1, x0);
d011 = DIRECT_A3D_ELEM(data, z0, y1, x1);
d100 = DIRECT_A3D_ELEM(data, z1, y0, x0);
d101 = DIRECT_A3D_ELEM(data, z1, y0, x1);
d110 = DIRECT_A3D_ELEM(data, z1, y1, x0);
d111 = DIRECT_A3D_ELEM(data, z1, y1, x1);
// Set the interpolated value in the 2D output array
// interpolate in x
#ifndef FLOAT_PRECISION
__m256d __fx = _mm256_set1_pd(fx);
__m256d __interpx1 = LIN_INTERP_AVX(_mm256_setr_pd(d000.real, d000.imag, d100.real, d100.imag),
_mm256_setr_pd(d001.real, d001.imag, d101.real, d101.imag),
__fx);
__m256d __interpx2 = LIN_INTERP_AVX(_mm256_setr_pd(d010.real, d010.imag, d110.real, d110.imag),
_mm256_setr_pd(d011.real, d011.imag, d111.real, d111.imag),
__fx);
// interpolate in y
__m256d __fy = _mm256_set1_pd(fy);
__m256d __interpy = LIN_INTERP_AVX(__interpx1, __interpx2, __fy);
#else
__m128 __fx = _mm_set1_ps(fx);
__m128 __interpx1 = LIN_INTERP_AVX(_mm_setr_ps(d000.real, d000.imag, d100.real, d100.imag),
_mm_setr_ps(d001.real, d001.imag, d101.real, d101.imag),
__fx);
__m128 __interpx2 = LIN_INTERP_AVX(_mm_setr_ps(d010.real, d010.imag, d110.real, d110.imag),
_mm_setr_ps(d011.real, d011.imag, d111.real, d111.imag),
__fx);
// interpolate in y
__m128 __fy = _mm_set1_ps(fy);
__m128 __interpy = LIN_INTERP_AVX(__interpx1, __interpx2, __fy);
#endif
Complex* interpy = (Complex*)&__interpy;
//interpolate in z
DIRECT_A3D_ELEM(f3d, k, i, x) = LIN_INTERP(fz, interpy[0], interpy[1]);
// Take complex conjugated for half with negative x
if (is_neg_x)
DIRECT_A3D_ELEM(f3d, k, i, x) = conj(DIRECT_A3D_ELEM(f3d, k, i, x));
} // endif TRILINEAR
else if (interpolator == NEAREST_NEIGHBOUR )
{
x0 = ROUND(xp);
y0 = ROUND(yp);
z0 = ROUND(zp);
if (x0 < 0)
DIRECT_A3D_ELEM(f3d, k, i, x) = conj(A3D_ELEM(data, -z0, -y0, -x0));
else
DIRECT_A3D_ELEM(f3d, k, i, x) = A3D_ELEM(data, z0, y0, x0);
} // endif NEAREST_NEIGHBOUR
else
REPORT_ERROR("Unrecognized interpolator in Projector::project");
} // endif x-loop
} // endif y-loop
} // endif z-loop
}
void FourierProjector::project(double rot, double tilt, double psi, const MultidimArray<double> *ctf)
{
double freqy, freqx;
std::complex< double > f;
Euler_angles2matrix(rot,tilt,psi,E);
projectionFourier.initZeros();
double maxFreq2=maxFrequency*maxFrequency;
int Xdim=(int)XSIZE(VfourierRealCoefs);
int Ydim=(int)YSIZE(VfourierRealCoefs);
int Zdim=(int)ZSIZE(VfourierRealCoefs);
for (size_t i=0; i<YSIZE(projectionFourier); ++i)
{
FFT_IDX2DIGFREQ(i,volumeSize,freqy);
double freqy2=freqy*freqy;
double freqYvol_X=MAT_ELEM(E,1,0)*freqy;
double freqYvol_Y=MAT_ELEM(E,1,1)*freqy;
double freqYvol_Z=MAT_ELEM(E,1,2)*freqy;
for (size_t j=0; j<XSIZE(projectionFourier); ++j)
{
// The frequency of pairs (i,j) in 2D
FFT_IDX2DIGFREQ(j,volumeSize,freqx);
// Do not consider pixels with high frequency
if ((freqy2+freqx*freqx)>maxFreq2)
continue;
// Compute corresponding frequency in the volume
double freqvol_X=freqYvol_X+MAT_ELEM(E,0,0)*freqx;
double freqvol_Y=freqYvol_Y+MAT_ELEM(E,0,1)*freqx;
double freqvol_Z=freqYvol_Z+MAT_ELEM(E,0,2)*freqx;
double c,d;
if (BSplineDeg==0)
{
// 0 order interpolation
// Compute corresponding index in the volume
int kVolume=(int)round(freqvol_Z*volumePaddedSize);
int iVolume=(int)round(freqvol_Y*volumePaddedSize);
int jVolume=(int)round(freqvol_X*volumePaddedSize);
c = A3D_ELEM(VfourierRealCoefs,kVolume,iVolume,jVolume);
d = A3D_ELEM(VfourierImagCoefs,kVolume,iVolume,jVolume);
}
else if (BSplineDeg==1)
{
// B-spline linear interpolation
double kVolume=freqvol_Z*volumePaddedSize;
double iVolume=freqvol_Y*volumePaddedSize;
double jVolume=freqvol_X*volumePaddedSize;
c=VfourierRealCoefs.interpolatedElement3D(jVolume,iVolume,kVolume);
d=VfourierImagCoefs.interpolatedElement3D(jVolume,iVolume,kVolume);
}
else
{
// B-spline cubic interpolation
double kVolume=freqvol_Z*volumePaddedSize;
double iVolume=freqvol_Y*volumePaddedSize;
double jVolume=freqvol_X*volumePaddedSize;
// Commented for speed-up, the corresponding code is below
// c=VfourierRealCoefs.interpolatedElementBSpline3D(jVolume,iVolume,kVolume);
// d=VfourierImagCoefs.interpolatedElementBSpline3D(jVolume,iVolume,kVolume);
// The code below is a replicate for speed reasons of interpolatedElementBSpline3D
double z=kVolume;
double y=iVolume;
double x=jVolume;
// Logical to physical
z -= STARTINGZ(VfourierRealCoefs);
y -= STARTINGY(VfourierRealCoefs);
x -= STARTINGX(VfourierRealCoefs);
int l1 = (int)ceil(x - 2);
int l2 = l1 + 3;
int m1 = (int)ceil(y - 2);
int m2 = m1 + 3;
int n1 = (int)ceil(z - 2);
int n2 = n1 + 3;
c = d = 0.0;
double aux;
for (int nn = n1; nn <= n2; nn++)
{
int equivalent_nn=nn;
if (nn<0)
equivalent_nn=-nn-1;
else if (nn>=Zdim)
equivalent_nn=2*Zdim-nn-1;
double yxsumRe = 0.0, yxsumIm = 0.0;
for (int m = m1; m <= m2; m++)
{
int equivalent_m=m;
if (m<0)
equivalent_m=-m-1;
else if (m>=Ydim)
equivalent_m=2*Ydim-m-1;
double xsumRe = 0.0, xsumIm = 0.0;
for (int l = l1; l <= l2; l++)
{
double xminusl = x - (double) l;
int equivalent_l=l;
if (l<0)
equivalent_l=-l-1;
else if (l>=Xdim)
equivalent_l=2*Xdim-l-1;
double CoeffRe = (double) DIRECT_A3D_ELEM(VfourierRealCoefs,equivalent_nn,equivalent_m,equivalent_l);
double CoeffIm = (double) DIRECT_A3D_ELEM(VfourierImagCoefs,equivalent_nn,equivalent_m,equivalent_l);
BSPLINE03(aux,xminusl);
xsumRe += CoeffRe * aux;
xsumIm += CoeffIm * aux;
}
double yminusm = y - (double) m;
BSPLINE03(aux,yminusm);
yxsumRe += xsumRe * aux;
yxsumIm += xsumIm * aux;
}
double zminusn = z - (double) nn;
BSPLINE03(aux,zminusn);
c += yxsumRe * aux;
d += yxsumIm * aux;
}
}
// Phase shift to move the origin of the image to the corner
double a=DIRECT_A2D_ELEM(phaseShiftImgA,i,j);
double b=DIRECT_A2D_ELEM(phaseShiftImgB,i,j);
if (ctf!=NULL)
{
double ctfij=DIRECT_A2D_ELEM(*ctf,i,j);
a*=ctfij;
b*=ctfij;
}
// Multiply Fourier coefficient in volume times phase shift
double ac = a * c;
double bd = b * d;
double ab_cd = (a + b) * (c + d);
// And store the multiplication
double *ptrI_ij=(double *)&DIRECT_A2D_ELEM(projectionFourier,i,j);
*ptrI_ij = ac - bd;
*(ptrI_ij+1) = ab_cd - ac - bd;
}
}
transformer2D.inverseFourierTransform();
}
void ProgResolutionIBW::edgeWidth(const MultidimArray<double> &volCoeffs,
const MultidimArray<double> &edges,
MultidimArray <double>& widths, const Matrix1D<double> &dir,
double step) const
{
double forward_count, backward_count, slope;
Matrix1D<double> pos_aux_fw(3), pos_aux_bw(3), pos(3), pos_aux(3), next_pos(3), Kdir;
Kdir=step*dir;
//Visit all elements in volume
FOR_ALL_ELEMENTS_IN_ARRAY3D(edges)
{
//Check for border pixels
if (A3D_ELEM(edges,k,i,j)!=0)
{
//reset all counters
forward_count=0;
backward_count=0;
VECTOR_R3(pos_aux_fw,j,i,k);
pos_aux_bw=pos=pos_aux_fw;
//find out if pixel magnitude grows or decreases
pos_aux=pos;
pos_aux+=dir;
double value_plus_dir=volCoeffs.interpolatedElementBSpline3D(XX(pos_aux),YY(pos_aux),ZZ(pos_aux));
pos_aux=pos;
pos_aux-=dir;
double value_minus_dir=volCoeffs.interpolatedElementBSpline3D(XX(pos_aux),YY(pos_aux),ZZ(pos_aux));
slope=value_plus_dir-value_minus_dir;
double sign;
if (slope>0)
sign=1;
else
sign=-1;
//current_pixel is multiplied by the sign, so only one condition is enough to detect an
//extremum no matter if the pixel values increase or decrease
double current_pixel=sign*volCoeffs.interpolatedElementBSpline3D
(XX(pos_aux_fw),YY(pos_aux_fw),ZZ(pos_aux_fw));
double next_pixel;
bool not_found;
//Search for local extremum ahead of the edge in the given direction
do
{
not_found=true;
next_pos=pos_aux_fw+Kdir;
next_pixel=sign*volCoeffs.interpolatedElementBSpline3D
(XX(next_pos),YY(next_pos),ZZ(next_pos));
if(next_pixel>current_pixel)
{
current_pixel=next_pixel;
pos_aux_fw=next_pos;
forward_count++;
}
else
{
not_found=false;
}
}
while(not_found);
current_pixel=sign*volCoeffs.interpolatedElementBSpline3D
(XX(pos_aux_bw),YY(pos_aux_bw),ZZ(pos_aux_bw));
//Search for local extremum behind of the edge in the given direction
do
{
not_found=true;
next_pos=pos_aux_bw-Kdir;
next_pixel=sign*volCoeffs.interpolatedElementBSpline3D
(XX(next_pos),YY(next_pos),ZZ(next_pos));
if(next_pixel<current_pixel)
{
current_pixel=next_pixel;
pos_aux_bw=next_pos;
backward_count++;
}
else
{
not_found=false;
}
}
while(not_found);
//If the width found for this position is smaller than the one stores in edges volume
//before it is overwritten
if ((forward_count+backward_count)<A3D_ELEM(widths,k,i,j))
{
A3D_ELEM(widths,k,i,j)=forward_count+backward_count;
}
}
}
}
// Evaluate plane ----------------------------------------------------------
double evaluatePlane(double rot, double tilt,
const MultidimArray<double> *V, const MultidimArray<double> *Vmag,
double maxFreq, double planeWidth, int direction,
MultidimArray<double> *Vdraw=NULL,
bool setPos=false, double rotPos=0, double tiltPos=0)
{
if (rot<0 || rot>360 || tilt<-90 || tilt>90)
return 0;
Matrix2D<double> E, Einv;
Euler_angles2matrix(rot,tilt,0,E);
Einv=E.transpose();
if (setPos)
{
Matrix2D<double> Epos;
Euler_angles2matrix(rotPos,tiltPos,0,Epos);
double angle=acos(E(2,0)*Epos(2,0)+E(2,1)*Epos(2,1)+E(2,2)*Epos(2,2));
angle=RAD2DEG(angle);
if (fabs(angle)<20 || fabs(180-angle)<20)
return 0;
}
size_t N=XMIPP_MAX(XSIZE(*Vmag),YSIZE(*Vmag)/2);
N=XMIPP_MAX(N,ZSIZE(*Vmag)/2);
double df=0.5/N;
Matrix1D<double> freq(3), freqp(3);
Matrix1D<int> idx(3);
double sumNeg=0, sumPos=0;
int Nneg=0, Npos=0;
double maxFreq2=maxFreq*maxFreq;
int iPlaneWidth=(int)ceil(planeWidth);
for (double ix=0; ix<=N; ix++)
{
XX(freq)=ix*df;
double fx2=XX(freq)*XX(freq);
if (fx2>maxFreq2)
continue;
for (double iy=-(int)N; iy<=N; iy++)
{
YY(freq)=iy*df;
double fx2fy2=fx2+YY(freq)*YY(freq);
if (fx2fy2>maxFreq2)
continue;
for (int iz=-iPlaneWidth; iz<=iPlaneWidth; iz++)
{
if (iz==0 || ix==0 || iy==0)
continue;
// Frequency in the coordinate system of the plane
ZZ(freq)=iz*df;
// Frequency in the coordinate system of the volume
SPEED_UP_temps012;
M3x3_BY_V3x1(freqp,Einv,freq);
bool inverted=false;
if (XX(freqp)<0)
{
XX(freqp)=-XX(freqp);
YY(freqp)=-YY(freqp);
ZZ(freqp)=-ZZ(freqp);
inverted=true;
}
// Get the corresponding index
DIGFREQ2FFT_IDX(ZZ(freqp), ZSIZE(*V), ZZ(idx));
DIGFREQ2FFT_IDX(YY(freqp), YSIZE(*V), YY(idx));
DIGFREQ2FFT_IDX(XX(freqp), XSIZE(*V), XX(idx));
if (XX(idx) < STARTINGX(*Vmag) || XX(idx) > FINISHINGX(*Vmag) ||
YY(idx) < STARTINGY(*Vmag) || YY(idx) > FINISHINGY(*Vmag) ||
ZZ(idx) < STARTINGZ(*Vmag) || ZZ(idx) > FINISHINGZ(*Vmag))
continue;
// Make the corresponding sums
bool negativeSum;
if (direction==1)
negativeSum=iz<0;
else
negativeSum=iz>0;
double val=A3D_ELEM(*Vmag,ZZ(idx),YY(idx),XX(idx));
if ((negativeSum && !inverted) || (!negativeSum && inverted)) // XOR
{
sumNeg+=val;
Nneg++;
if (Vdraw!=NULL)
(*Vdraw)(idx)=2*direction*val;
}
else
{
sumPos+=val;
Npos++;
if (Vdraw!=NULL)
(*Vdraw)(idx)=1.0/2.0*direction*val;
}
}
}
}
if (fabs(Nneg-Npos)/(0.5*(Nneg+Npos))>0.5)
// If there is a difference of more than 50%
return 1e38;
if (Nneg!=0)
sumNeg/=Nneg;
else
return 1e38;
if (Npos!=0)
sumPos/=Npos;
else
return 1e38;
return -(sumPos-sumNeg);
}
void * thread_process_plane( void * args )
{
ThreadProcessPlaneArgs * thrParams = (ThreadProcessPlaneArgs *) args;
unsigned int myID = thrParams->myID;
unsigned int numThreads = thrParams->numThreads;
double sigma_s = thrParams->sigma_s;
double sigma_r = thrParams->sigma_r;
MultidimArray<double> &input = *thrParams->input;
MultidimArray<double> &output = *thrParams->output;
bool fast = thrParams->fast;
if( !fast )
{
sigma_s /= 3.0;
sigma_s = CEIL( sigma_s );
sigma_r /= 3.0;
}
else
{
sigma_s = CEIL( sigma_s );
}
double curr_I;
double inv_2_sigma_s_2 = 1.0/( 2.0* sigma_s * sigma_s);
double inv_2_sigma_r_2 = 1.0/( 2.0* sigma_r * sigma_r);
double sigma_r_2 = sigma_r * sigma_r;
int _3_sigma_s = (int)(3 * sigma_s);
double _3_sigma_r = 3.0 * sigma_r;
int y_min = input.startingY();
int y_max = input.finishingY();
int x_min = input.startingX();
int x_max = input.finishingX();
int z_min = input.startingZ();
int z_max = input.finishingZ();
int curr_x, curr_y, curr_z;
int prev_x, prev_y, prev_z;
double error;
int myFirstY, myLastY;
int numElems = y_max-y_min+1;
numElems /= numThreads;
myFirstY = myID * numElems + y_min;
if( myID == numThreads -1 )
myLastY = y_max;
else
myLastY = myFirstY + numElems - 1;
if( myID == 0 )
{
std::cerr << "Progress (over a total of " << z_max - z_min + 1 << " slices): ";
}
if( fast )
{
for (int k=z_min; k<=z_max; k++)
{
if( myID == 0 )
{
if( z_min < 0 )
std::cerr << k - z_min << " ";
else
std::cerr << k + z_min << " ";
}
for (int i=myFirstY; i<=myLastY; i++)
{
for (int j=x_min; j<=x_max; j++)
{
// x == j
// y == i
// z == k
int xc = j;
int yc = i;
int zc = k;
int xcOld, ycOld, zcOld;
double YcOld=0;
double Yc = A3D_ELEM( input, k, i, j);
int iters =0;
double shift;
int num;
double mY;
do
{
xcOld = xc;
ycOld = yc;
zcOld = zc;
double mx=0;
double my=0;
double mz=0;
mY=0;
num=0;
for( int z_j = -(int)sigma_s ; z_j <=(int)sigma_s ; z_j++ )
{
int z2 = zc + z_j;
if( z2 >= z_min && z2 <= z_max)
{
int z_j_2 = z_j * z_j; // Speed-up
for( int y_j = -(int)sigma_s ; y_j <= (int)sigma_s ; y_j++ )
{
int y2 = yc + y_j;
if( y2 >= y_min && y2 <= y_max)
{
int y_j_2 = y_j * y_j; // Speed-up
for( int x_j = -(int)sigma_s ; x_j <= (int)sigma_s ; x_j++ )
{
int x2 = xc + x_j;
if( x2 >= x_min && x2 <= x_max)
{
if( x_j*x_j+y_j_2+z_j_2 <= sigma_s*sigma_s )
{
double Y2 = A3D_ELEM( input, z2, y2, x2);
double dY = Yc - Y2;
if( dY*dY <= sigma_r_2 )
{
mx += x2;
my += y2;
mz += z2;
mY += Y2;
num++;
}
}
}
}
}
}
}
}
double num_ = 1.0/num;
Yc = mY*num_;
xc = (int) (mx*num_+0.5);
yc = (int) (my*num_+0.5);
zc = (int) (mz*num_+0.5);
int dx = xc-xcOld;
int dy = yc-ycOld;
int dz = zc-zcOld;
double dY = Yc-YcOld;
shift = dx*dx+dy*dy+dz*dz*dY*dY;
iters++;
}
while(shift>3 && iters<100);
A3D_ELEM( output, k,i,j ) = Yc;
}
}
}
}
else
{
for (int k=z_min; k<=z_max; k++)
{
if( myID == 0 )
std::cerr << k << " ";
for (int i=myFirstY; i<=myLastY; i++)
{
for (int j=x_min; j<=x_max; j++)
{
// x == j
// y == i
// z == k
curr_x = j;
curr_y = i;
curr_z = k;
curr_I = A3D_ELEM( input, k, i, j);
double sum_denom;
double I_sum;
do
{
// Check neighbourhood
double x_sum = 0, y_sum = 0, z_sum = 0;
sum_denom = 0;
I_sum = 0;
for( int z_j = curr_z - _3_sigma_s ; z_j <= curr_z + _3_sigma_s ; z_j++ )
{
if( z_j >= z_min && z_j <= z_max)
{
for( int y_j = curr_y - _3_sigma_s ; y_j <= curr_y + _3_sigma_s ; y_j++ )
{
if( y_j >= y_min && y_j <= y_max)
{
for( int x_j = curr_x - _3_sigma_s ; x_j <= curr_x + _3_sigma_s ; x_j++ )
{
if( x_j >= x_min && x_j <= x_max)
{
double I_j = A3D_ELEM( input, z_j, y_j, x_j);
double I_dist=fabs(I_j - curr_I);
if( I_dist <= _3_sigma_r )
{
// Take this point into account
double eucl_dist = (curr_x - x_j)*(curr_x - x_j)+(curr_y - y_j)*(curr_y - y_j)+(curr_z - z_j)*(curr_z - z_j);
double aux = exp(-(eucl_dist*inv_2_sigma_s_2) - (I_dist*I_dist*inv_2_sigma_r_2));
x_sum += x_j*aux;
y_sum += y_j*aux;
z_sum += z_j*aux;
I_sum += I_j*aux;
sum_denom += aux;
}
}
}
}
}
}
}
prev_x = (int)round(curr_x);
prev_y = (int)round(curr_y);
prev_z = (int)round(curr_z);
double isum_denom=1.0/sum_denom;
curr_x = (int)round(x_sum*isum_denom);
curr_y = (int)round(y_sum*isum_denom);
curr_z = (int)round(z_sum*isum_denom);
curr_I = I_sum*isum_denom;
error = fabs(prev_x - curr_x) + fabs(prev_y - curr_y) + fabs(prev_z - curr_z);
}
while(error>0);
A3D_ELEM( output, k,i,j ) = curr_I;
}
}
}
}
return NULL;
}
