/// Clears peaks and adds a peak for each hkl, all with the same FWHM and /// height. void PawleyFunction::setPeaks(const std::vector<Kernel::V3D> &hkls, double fwhm, double height) { clearPeaks(); for (const auto &hkl : hkls) { addPeak(hkl, fwhm, height); } }
/* * calcFit() * * Calculate fit from gaussian parameters. This assumes * that all the peak parameters are reasonable. */ void calcFit() { int i; fitData *cur; // zero matrices. for(i=0;i<(image_size_x*image_size_y);i++){ f_data[i] = 1.0; bg_counts[i] = 0; bg_data[i] = 0; } // update fit matrix with values from fits. for(i=0;i<nfit;i++){ cur = &fit[i]; if (cur->status != ERROR){ addPeak(cur); } } }
void PoldiPeakCollection::setPeaks(const std::vector<V3D> &hkls, const std::vector<double> &dValues, const std::vector<double> &fSquared) { if (hkls.size() != dValues.size()) { throw std::invalid_argument( "hkl-vector and d-vector do not have the same length."); } if (!m_pointGroup) { throw std::runtime_error("Cannot set peaks without point group."); } m_peaks.clear(); for (size_t i = 0; i < hkls.size(); ++i) { double multiplicity = static_cast<double>(m_pointGroup->getEquivalents(hkls[i]).size()); addPeak(PoldiPeak::create( MillerIndices(hkls[i]), UncertainValue(dValues[i]), UncertainValue(multiplicity * fSquared[i]), UncertainValue(0.0))); } }
/* * newPeaks() * * Create initial fit data structures for spline fitting from the peak array. The * format of the initial values is the same as what was used for 3D-DAOSTORM. * * peaks - pointer to the initial values for the parameters for each peak. * 1. height * 2. x-center * 3. x-sigma * 4. y-center * 5. y-sigma * 6. background * 7. z-center * 8. status * 9. error * .. repeat .. * n_peaks - The number of parameters (peaks). */ void newPeaks(double *peaks, int n_peaks, int fit_type) { int i,j,n_fit_params,sx,sy,sz; double temp; if(fit_type == F2D){ n_fit_params = 4; } else{ n_fit_params = 5; } /* Delete old fit data structures, if they exist. */ freePeaks(); /* Reset the fit array. */ resetFit(); /* Allocate storage for fit data structure. */ n_fit_data = n_peaks; new_fit_data = (fitData *)malloc(sizeof(fitData)*n_peaks); old_fit_data = (fitData *)malloc(sizeof(fitData)*n_peaks); sx = getXSize()/2 - 1; sy = getYSize()/2 - 1; sz = getZSize(); /* Note the assumption here that the splines are square. */ if((sx%2)==1){ xoff = (double)(sx/2) - 1.0; yoff = (double)(sy/2) - 1.0; } else{ xoff = (double)(sx/2) - 1.5; yoff = (double)(sy/2) - 1.5; } //zoff = -(double)(sz/2); zoff = 0.0; /* Initialize fit data structure. */ for(i=0;i<n_fit_data;i++){ new_fit_data[i].index = i; new_fit_data[i].n_params = n_fit_params; new_fit_data[i].size_x = sx; new_fit_data[i].size_y = sy; new_fit_data[i].size_z = sz; new_fit_data[i].status = (int)(peaks[i*NRESULTSPAR+STATUS]); new_fit_data[i].type = fit_type; new_fit_data[i].lambda = 1.0; mallocFitData(&(new_fit_data[i]), n_fit_params, sx*sy*(n_fit_params+1)); for(j=0;j<n_fit_params;j++){ new_fit_data[i].sign[j] = 0; } new_fit_data[i].clamp[CF_HEIGHT] = 100.0; new_fit_data[i].clamp[CF_XCENTER] = 1.0; new_fit_data[i].clamp[CF_YCENTER] = 1.0; new_fit_data[i].clamp[CF_BACKGROUND] = 20.0; if (fit_type == F3D){ new_fit_data[i].clamp[CF_ZCENTER] = 2.0; } if (DEBUG){ printf(" peaks: %.2f %.2f %.2f\n", peaks[i*NRESULTSPAR+XCENTER], peaks[i*NRESULTSPAR+YCENTER], peaks[i*NRESULTSPAR+ZCENTER]); } new_fit_data[i].params[CF_HEIGHT] = peaks[i*NRESULTSPAR+HEIGHT]; new_fit_data[i].params[CF_XCENTER] = peaks[i*NRESULTSPAR+XCENTER] - xoff; new_fit_data[i].params[CF_YCENTER] = peaks[i*NRESULTSPAR+YCENTER] - yoff; new_fit_data[i].params[CF_BACKGROUND] = peaks[i*NRESULTSPAR+BACKGROUND]; if (fit_type == F3D){ new_fit_data[i].params[CF_ZCENTER] = peaks[i*NRESULTSPAR+ZCENTER] - zoff; } new_fit_data[i].xi = (int)(new_fit_data[i].params[CF_XCENTER]); new_fit_data[i].yi = (int)(new_fit_data[i].params[CF_YCENTER]); if (fit_type == F3D){ new_fit_data[i].zi = (int)(new_fit_data[i].params[CF_ZCENTER]); } if (fit_type == F2D){ updateFitValues2D(&(new_fit_data[i])); } else{ updateFitValues3D(&(new_fit_data[i])); } /* * The derivative with respect to background term is always 1.0 */ for(j=0;j<(sx*sy);j++){ new_fit_data[i].values[(CF_BACKGROUND+1)*sx*sy+j] = 1.0; } addPeak(&(new_fit_data[i])); mallocFitData(&(old_fit_data[i]), n_fit_params, sx*sy*(n_fit_params+1)); copyFitData(&(new_fit_data[i]), &(old_fit_data[i])); if(new_fit_data[i].status == RUNNING){ if (TESTING){ temp = calcError(&(new_fit_data[i]), new_fit_data[i].params[CF_BACKGROUND]); new_fit_data[i].error = temp; } else{ new_fit_data[i].error = 1.0e+12; } old_fit_data[i].error = 1.0e+13; } else{ new_fit_data[i].error = peaks[i*NRESULTSPAR+IERROR]; old_fit_data[i].error = new_fit_data[i].error; } } }
/* * iterateSpline() * * Perform one cycle of update of all the peaks. */ void iterateSpline(void) { int i; fitData *new_peak, *old_peak; for(i=0;i<n_fit_data;i++){ new_peak = &(new_fit_data[i]); old_peak = &(old_fit_data[i]); if (new_peak->status == RUNNING){ /* Add the peak & calculate the error. */ new_peak->error = calcError(new_peak, new_peak->params[CF_BACKGROUND]); //printf("error: %f %f %f\n", new_peak->error, old_peak->error, new_peak->lambda); if((fabs(new_peak->error - old_peak->error)/new_peak->error) < tolerance){ new_peak->status = CONVERGED; } else{ /* * Check if the fit is getting worse. If it is then reset to the * previous parameters & increase the lambda parameter. This will * cause the search to move downhill with higher probability but * less speed. */ if(new_peak->error > (1.5*old_peak->error)){ subtractPeak(new_peak); copyFitData(old_peak, new_peak); new_peak->lambda = 10.0 * new_peak->lambda; addPeak(new_peak); } /* * If the fit is getting better and lambda is greater than 1.0 * then we slowly decrease lambda. */ else if(new_peak->error < old_peak->error){ if(new_peak->lambda > 1.0){ new_peak->lambda = 0.8*new_peak->lambda; if(new_peak->lambda < 1.0){ new_peak->lambda = 1.0; } } } /* Calculate updated fit values. */ updateFit(new_peak, new_peak->params[CF_BACKGROUND]); /* * Increase lambda in the event of Cholesky failure. */ if(new_peak->status == CHOLERROR){ new_peak->lambda = 10.0 * new_peak->lambda; new_peak->status = RUNNING; } else{ subtractPeak(new_peak); copyFitData(new_peak, old_peak); updatePeakParameters(new_peak); if(new_peak->type == F2D){ updateFitValues2D(new_peak); } else{ updateFitValues3D(new_peak); } if(new_peak->status != BADPEAK){ addPeak(new_peak); } } } } } }
/* * update2D() * * Update current fits given equal width in x and y. * * This procedure is also responsible for flagging peaks * that might be bad & that should be removed from fitting. */ void update2D() { // Lapack int n = 5, nrhs = 1, lda = 5, ldb = 5, info; // Local int i,j,k,l,m,wx,wy; double fi,xi,xt,ext,yt,eyt,e_t,t1,t2,a1,width; double delta[NPEAKPAR]; double jt[5]; double jacobian[5]; double hessian[25]; fitData *cur; for(i=0;i<NPEAKPAR;i++){ delta[i] = 0.0; } for(i=0;i<nfit;i++){ cur = &fit[i]; if(cur->status==RUNNING){ for(j=0;j<5;j++){ jacobian[j] = 0.0; } for(j=0;j<25;j++){ hessian[j] = 0.0; } l = cur->offset; wx = cur->wx; wy = cur->wy; a1 = cur->params[HEIGHT]; width = cur->params[XWIDTH]; for(j=-wy;j<=wy;j++){ yt = cur->yt[j+wy]; eyt = cur->eyt[j+wy]; for(k=-wx;k<=wx;k++){ m = j*image_size_x+k+l; fi = f_data[m]+bg_data[m]/((double)bg_counts[m]); xi = x_data[m]; xt = cur->xt[k+wx]; ext = cur->ext[k+wx]; e_t = ext*eyt; jt[0] = e_t; jt[1] = 2.0*a1*width*xt*e_t; jt[2] = 2.0*a1*width*yt*e_t; jt[3] = -a1*xt*xt*e_t-a1*yt*yt*e_t; jt[4] = 1.0; // calculate jacobian t1 = 2.0*(1.0 - xi/fi); jacobian[0] += t1*jt[0]; jacobian[1] += t1*jt[1]; jacobian[2] += t1*jt[2]; jacobian[3] += t1*jt[3]; jacobian[4] += t1*jt[4]; // calculate hessian t2 = 2.0*xi/(fi*fi); // calculate hessian without second derivative terms. hessian[0] += t2*jt[0]*jt[0]; hessian[1] += t2*jt[0]*jt[1]; hessian[2] += t2*jt[0]*jt[2]; hessian[3] += t2*jt[0]*jt[3]; hessian[4] += t2*jt[0]*jt[4]; // hessian[5] hessian[6] += t2*jt[1]*jt[1]; hessian[7] += t2*jt[1]*jt[2]; hessian[8] += t2*jt[1]*jt[3]; hessian[9] += t2*jt[1]*jt[4]; // hessian[10] // hessian[11] hessian[12] += t2*jt[2]*jt[2]; hessian[13] += t2*jt[2]*jt[3]; hessian[14] += t2*jt[2]*jt[4]; // hessian[15] // hessian[16] // hessian[17] hessian[18] += t2*jt[3]*jt[3]; hessian[19] += t2*jt[3]*jt[4]; // hessian[20] // hessian[21] // hessian[22] // hessian[23] hessian[24] += t2*jt[4]*jt[4]; } } // subtract the old peak out of the foreground and background arrays. subtractPeak(cur); // Use Lapack to solve AX=B to calculate update vector dposv_( "Lower", &n, &nrhs, hessian, &lda, jacobian, &ldb, &info ); if(info!=0){ cur->status = ERROR; if(TESTING){ printf("fitting error! %d %d %d\n", i, info, ERROR); printf(" %f %f %f %f %f\n", delta[HEIGHT], delta[XCENTER], delta[YCENTER], delta[XWIDTH], delta[BACKGROUND]); } } else{ // update params delta[HEIGHT] = jacobian[0]; delta[XCENTER] = jacobian[1]; delta[YCENTER] = jacobian[2]; delta[XWIDTH] = jacobian[3]; delta[YWIDTH] = jacobian[3]; delta[BACKGROUND] = jacobian[4]; fitDataUpdate(cur, delta); // add the new peak to the foreground and background arrays. // recalculate peak fit area as the peak width may have changed. if (cur->status != ERROR){ cur->wx = calcWidth(cur->params[XWIDTH],cur->wx); cur->wy = cur->wx; addPeak(cur); } } } } }
/* * updateZ() * * Update current fits given x, y width determined by z parameter. * * This procedure is also responsible for flagging peaks * that might be bad & that should be removed from fitting. */ void updateZ() { // Lapack int n = 5, nrhs = 1, lda = 5, ldb = 5, info; // Local int i,j,k,l,m,wx,wy; double fi,xi,xt,ext,yt,eyt,e_t,t1,t2,a1,a3,a5; double z0,z1,z2,zt,gx,gy; double delta[NPEAKPAR]; double jt[5]; double jacobian[5]; double hessian[25]; fitData *cur; for(i=0;i<NPEAKPAR;i++){ delta[i] = 0.0; } for(i=0;i<nfit;i++){ cur = &fit[i]; if(cur->status==RUNNING){ for(j=0;j<5;j++){ jacobian[j] = 0.0; } for(j=0;j<25;j++){ hessian[j] = 0.0; } l = cur->offset; wx = cur->wx; wy = cur->wy; a1 = cur->params[HEIGHT]; a3 = cur->params[XWIDTH]; a5 = cur->params[YWIDTH]; // dwx/dz calcs z0 = (cur->params[ZCENTER]-wx_z_params[1])/wx_z_params[2]; z1 = z0*z0; z2 = z1*z0; zt = 2.0*z0+3.0*wx_z_params[3]*z1+4.0*wx_z_params[4]*z2; gx = -2.0*zt/(wx_z_params[0]*cur->wx_term); // dwy/dz calcs z0 = (cur->params[ZCENTER]-wy_z_params[1])/wy_z_params[2]; z1 = z0*z0; z2 = z1*z0; zt = 2.0*z0+3.0*wy_z_params[3]*z1+4.0*wy_z_params[4]*z2; gy = -2.0*zt/(wy_z_params[0]*cur->wy_term); for(j=-wy;j<=wy;j++){ yt = cur->yt[j+wy]; eyt = cur->eyt[j+wy]; for(k=-wx;k<=wx;k++){ m = j*image_size_x+k+l; fi = f_data[m]+bg_data[m]/((double)bg_counts[m]); xi = x_data[m]; xt = cur->xt[k+wx]; ext = cur->ext[k+wx]; e_t = ext*eyt; // first derivatives jt[0] = e_t; jt[1] = 2.0*a1*a3*xt*e_t; jt[2] = 2.0*a1*a5*yt*e_t; jt[3] = -a1*xt*xt*gx*e_t-a1*yt*yt*gy*e_t; jt[4] = 1.0; // calculate jacobian t1 = 2.0*(1.0 - xi/fi); jacobian[0] += t1*jt[0]; jacobian[1] += t1*jt[1]; jacobian[2] += t1*jt[2]; jacobian[3] += t1*jt[3]; jacobian[4] += t1*jt[4]; // calculate hessian t2 = 2.0*xi/(fi*fi); // calculate hessian without second derivative terms. hessian[0] += t2*jt[0]*jt[0]; hessian[1] += t2*jt[0]*jt[1]; hessian[2] += t2*jt[0]*jt[2]; hessian[3] += t2*jt[0]*jt[3]; hessian[4] += t2*jt[0]*jt[4]; // hessian[5] hessian[6] += t2*jt[1]*jt[1]; hessian[7] += t2*jt[1]*jt[2]; hessian[8] += t2*jt[1]*jt[3]; hessian[9] += t2*jt[1]*jt[4]; // hessian[10] // hessian[11] hessian[12] += t2*jt[2]*jt[2]; hessian[13] += t2*jt[2]*jt[3]; hessian[14] += t2*jt[2]*jt[4]; // hessian[15] // hessian[16] // hessian[17] hessian[18] += t2*jt[3]*jt[3]; hessian[19] += t2*jt[3]*jt[4]; // hessian[20] // hessian[21] // hessian[22] // hessian[23] hessian[24] += t2*jt[4]*jt[4]; } } // subtract the old peak out of the foreground and background arrays. subtractPeak(cur); // Use Lapack to solve AX=B to calculate update vector dposv_( "Lower", &n, &nrhs, hessian, &lda, jacobian, &ldb, &info ); /* for(j=0;j<5;j++){ for(k=0;k<5;k++){ printf("%.4f ", hessian[j*5+k]); } printf("\n"); } */ if(info!=0){ cur->status = ERROR; if(TESTING){ printf("fitting error! %d %d %d\n", i, info, ERROR); } } else{ // update params delta[HEIGHT] = jacobian[0]; delta[XCENTER] = jacobian[1]; delta[YCENTER] = jacobian[2]; delta[ZCENTER] = jacobian[3]; delta[BACKGROUND] = jacobian[4]; fitDataUpdate(cur, delta); // add the new peak to the foreground and background arrays. if (cur->status != ERROR){ // calculate new x,y width, update fit area. calcWidthsFromZ(cur); cur->wx = calcWidth(cur->params[XWIDTH],cur->wx); cur->wy = calcWidth(cur->params[YWIDTH],cur->wy); addPeak(cur); } } } } if(VERBOSE){ printf("\n"); } }
/* * update3D() * * Update current fits allowing all parameters to change. * * This procedure is also responsible for flagging peaks * that might be bad & that should be removed from fitting. */ void update3D() { // Lapack int n = 6, nrhs = 1, lda = 6, ldb = 6, info; // Local int i,j,k,l,m,wx,wy; double fi,xi,xt,ext,yt,eyt,e_t,t1,t2,a1,a3,a5; double delta[NPEAKPAR]; double jt[6]; double jacobian[6]; double hessian[36]; fitData *cur; for(i=0;i<NPEAKPAR;i++){ delta[i] = 0.0; } for(i=0;i<nfit;i++){ cur = &fit[i]; if(cur->status==RUNNING){ for(j=0;j<6;j++){ jacobian[j] = 0.0; } for(j=0;j<36;j++){ hessian[j] = 0.0; } l = cur->offset; wx = cur->wx; wy = cur->wy; a1 = cur->params[HEIGHT]; a3 = cur->params[XWIDTH]; a5 = cur->params[YWIDTH]; for(j=-wy;j<=wy;j++){ yt = cur->yt[j+wy]; eyt = cur->eyt[j+wy]; for(k=-wx;k<=wx;k++){ m = j*image_size_x+k+l; fi = f_data[m]+bg_data[m]/((double)bg_counts[m]); xi = x_data[m]; xt = cur->xt[k+wx]; ext = cur->ext[k+wx]; e_t = ext*eyt; jt[0] = e_t; jt[1] = 2.0*a1*a3*xt*e_t; jt[2] = -a1*xt*xt*e_t; jt[3] = 2.0*a1*a5*yt*e_t; jt[4] = -a1*yt*yt*e_t; jt[5] = 1.0; // calculate jacobian t1 = 2.0*(1.0 - xi/fi); jacobian[0] += t1*jt[0]; jacobian[1] += t1*jt[1]; jacobian[2] += t1*jt[2]; jacobian[3] += t1*jt[3]; jacobian[4] += t1*jt[4]; jacobian[5] += t1*jt[5]; // calculate hessian t2 = 2.0*xi/(fi*fi); if (0){ // FIXME: not complete // hessian with second derivative terms. hessian[0] += t2*jt[0]*jt[0]; hessian[1] += t2*jt[0]*jt[1]+t1*2.0*xt*a3*ext*eyt; hessian[2] += t2*jt[0]*jt[2]; hessian[3] += t2*jt[0]*jt[3]+t1*2.0*yt*a5*ext*eyt; hessian[4] += t2*jt[0]*jt[4]; hessian[5] += t2*jt[0]*jt[5]; // hessian[6] hessian[7] += t2*jt[1]*jt[1]+t1*(-2.0*a1*a3*ext*eyt+4.0*a1*a3*a3*xt*xt*ext*eyt); hessian[8] += t2*jt[1]*jt[2]+t1*4.0*a1*xt*yt*a3*a3*ext*eyt; hessian[9] += t2*jt[1]*jt[3]; hessian[10] += t2*jt[1]*jt[4]; hessian[11] += t2*jt[1]*jt[5]; // hessian[12] // hessian[13] hessian[14] += t2*jt[2]*jt[2]+t1*(-2.0*a1*a3*ext*eyt+4.0*a1*a3*a3*yt*yt*ext*eyt); hessian[15] += t2*jt[2]*jt[3]; hessian[16] += t2*jt[2]*jt[4]; hessian[17] += t2*jt[2]*jt[5]; // hessian[18] // hessian[19] // hessian[20] hessian[21] += t2*jt[3]*jt[3]; hessian[22] += t2*jt[3]*jt[4]; hessian[23] += t2*jt[3]*jt[5]; // hessian[24] // hessian[25] // hessian[26] // hessian[27] hessian[28] += t2*jt[4]*jt[4]; hessian[29] += t2*jt[4]*jt[5]; // hessian[30] // hessian[31] // hessian[32] // hessian[33] // hessian[34] hessian[35] += t2*jt[5]*jt[5]; } else { // hessian without second derivative terms. hessian[0] += t2*jt[0]*jt[0]; hessian[1] += t2*jt[0]*jt[1]; hessian[2] += t2*jt[0]*jt[2]; hessian[3] += t2*jt[0]*jt[3]; hessian[4] += t2*jt[0]*jt[4]; hessian[5] += t2*jt[0]*jt[5]; // hessian[6] hessian[7] += t2*jt[1]*jt[1]; hessian[8] += t2*jt[1]*jt[2]; hessian[9] += t2*jt[1]*jt[3]; hessian[10] += t2*jt[1]*jt[4]; hessian[11] += t2*jt[1]*jt[5]; // hessian[12] // hessian[13] hessian[14] += t2*jt[2]*jt[2]; hessian[15] += t2*jt[2]*jt[3]; hessian[16] += t2*jt[2]*jt[4]; hessian[17] += t2*jt[2]*jt[5]; // hessian[18] // hessian[19] // hessian[20] hessian[21] += t2*jt[3]*jt[3]; hessian[22] += t2*jt[3]*jt[4]; hessian[23] += t2*jt[3]*jt[5]; // hessian[24] // hessian[25] // hessian[26] // hessian[27] hessian[28] += t2*jt[4]*jt[4]; hessian[29] += t2*jt[4]*jt[5]; // hessian[30] // hessian[31] // hessian[32] // hessian[33] // hessian[34] hessian[35] += t2*jt[5]*jt[5]; // Ignore off-diagonal terms. // This approach converges incredibly slowly. /* hessian[0] += t2*jt[0]*jt[0]; hessian[7] += t2*jt[1]*jt[1]; hessian[14] += t2*jt[2]*jt[2]; hessian[21] += t2*jt[3]*jt[3]; hessian[28] += t2*jt[4]*jt[4]; hessian[35] += t2*jt[5]*jt[5]; */ } } } /* printf("hessian:\n"); for(j=0;j<6;j++){ for(k=0;k<6;k++){ printf("%.4f ", hessian[j*6+k]); } printf("\n"); } printf("\n"); */ // subtract the old peak out of the foreground and background arrays. subtractPeak(cur); // Use Lapack to solve AX=B to calculate update vector dposv_( "Lower", &n, &nrhs, hessian, &lda, jacobian, &ldb, &info ); if(info!=0){ cur->status = ERROR; if(TESTING){ printf("fitting error! %d %d %d\n", i, info, ERROR); } } else{ // update params delta[HEIGHT] = jacobian[0]; delta[XCENTER] = jacobian[1]; delta[XWIDTH] = jacobian[2]; delta[YCENTER] = jacobian[3]; delta[YWIDTH] = jacobian[4]; delta[BACKGROUND] = jacobian[5]; fitDataUpdate(cur, delta); // add the new peak to the foreground and background arrays. if (cur->status != ERROR){ cur->wx = calcWidth(cur->params[XWIDTH],cur->wx); cur->wy = calcWidth(cur->params[YWIDTH],cur->wy); addPeak(cur); } } } } }