// 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; }