/** Find the correlation shift between the regularly spaces series frames and tracking,
* with a spacing of resolution.
*
* the returned shift is shift frames-tracking: frame = tracking + shift.
*/
double TemporalCalibration::findCorrelationShift(std::vector<double> frames, std::vector<double> tracking, double resolution) const
{
size_t N = std::min(tracking.size(), frames.size());
std::vector<double> result(N, 0);
correlate(&*frames.begin(), &*tracking.begin(), &*result.begin(), N / 2, N);
int top = std::distance(result.begin(), std::max_element(result.begin(), result.end()));
double shift = (N/2-top) * resolution; // convert to shift in ms.
mDebugStream << "=======================================" << std::endl;
mDebugStream << "tracking vs frames correlation:" << std::endl;
mDebugStream << "Temporal resolution " << resolution << " ms" << std::endl;
mDebugStream << "#frames=" << frames.size() << ", #tracks=" << tracking.size() << std::endl;
mDebugStream << std::endl;
mDebugStream << "Frame pos" << "\t" << "Track pos" << "\t" << "correlation" << std::endl;
for (int x = 0; x < N; ++x)
{
mDebugStream << frames[x] << "\t" << tracking[x] << "\t" << result[x] << std::endl;
}
mDebugStream << std::endl;
mDebugStream << "corr top: " << top << ", = shift in ms: " << shift << std::endl;
mDebugStream << "=======================================" << std::endl;
return shift; // shift frames-tracking: frame = tracking + shift
}
// correlate binaryish with vector
Eigen::MatrixXf multi_correlate(std::vector<float> & y_data, float * x_data, int x_vector_size, int size, int stride, int offset){
std::vector<float> sum(x_vector_size + 1);
std::vector<float> sum_of_squares(x_vector_size + 1);
int skipped = sum_calc(x_data, x_vector_size, size, sum.data(), sum_of_squares.data(), stride, offset);
skipped += sum_calc(y_data.data(), 1, size, &sum[x_vector_size], &sum_of_squares[x_vector_size]);
//qDebug() << "skipped: " << skipped << "size: " << size;
Eigen::MatrixXf sum_productmat(x_vector_size + 1, x_vector_size + 1);
sum_productmat_calc(y_data.data(), x_data, x_vector_size, size, sum_productmat, stride, offset);
Eigen::MatrixXf correlation_mat(x_vector_size + 1, x_vector_size + 1);
correlation_mat.setIdentity();
for(int r = 0; r < x_vector_size + 1; r++){
for(int c = r+1; c < x_vector_size + 1; c++){
// qDebug() << r << c << ": " << sum[c] << sum_of_squares[c] << sum[r] << sum_of_squares[r] << sum_productmat(r, c);
correlation_mat(r, c) = correlate(sum[c], sum_of_squares[c], sum[r], sum_of_squares[r], sum_productmat(r, c), size-skipped);
correlation_mat(c, r) = correlation_mat(r, c);
}
}
return correlation_mat;
}
bool generateRACHSequence(signalVector &gsmPulse,
int samplesPerSymbol)
{
if (gRACHSequence) {
if (gRACHSequence->sequence!=NULL) delete gRACHSequence->sequence;
if (gRACHSequence->sequenceReversedConjugated!=NULL) delete gRACHSequence->sequenceReversedConjugated;
}
signalVector *RACHSeq = modulateBurst(gRACHSynchSequence,
gsmPulse,
0,
samplesPerSymbol);
assert(RACHSeq);
signalVector *autocorr = correlate(RACHSeq,RACHSeq,NULL,NO_DELAY);
assert(autocorr);
gRACHSequence = new CorrelationSequence;
gRACHSequence->sequence = RACHSeq;
gRACHSequence->sequenceReversedConjugated = reverseConjugate(RACHSeq);
gRACHSequence->gain = peakDetect(*autocorr,&gRACHSequence->TOA,NULL);
delete autocorr;
return true;
}
double TemporalCalibration::findCorrelation(USFrameDataPtr data, int frame_a, int frame_b, double maxShift, double lastVal)
{
int maxShift_pix = maxShift / mFileData.mUsRaw->getSpacing()[1];
int lastVal_pix = lastVal / mFileData.mUsRaw->getSpacing()[1];
int line_index_x = mFileData.mProbeDefinition.mData.getOrigin_p()[0];
int dimY = mFileData.mUsRaw->getDimensions()[1];
vtkImageDataPtr line1 = this->extractLine_y(mFileData.mUsRaw, line_index_x, frame_a);
vtkImageDataPtr line2 = this->extractLine_y(mFileData.mUsRaw, line_index_x, frame_b);
int N = 2*dimY; //result vector allocate space on both sides of zero
std::vector<double> result(N, 0);
double* line_a = static_cast<double*>(line1->GetScalarPointer());
double* line_b = static_cast<double*>(line2->GetScalarPointer());
double* line_c = &*result.begin();
correlate(line_a, line_b, line_c, N/2, dimY);
// use the last found hit as a seed for looking for a local maximum
int lastTop = N/2 - lastVal_pix;
std::pair<int,int> range(lastTop - maxShift_pix, lastTop+maxShift_pix);
range.first = std::max(0, range.first);
range.second = std::min(N, range.second);
// look for a max in the vicinity of the last hit
int top = std::distance(result.begin(), std::max_element(result.begin()+range.first, result.begin()+range.second));
double hit = (N/2-top) * mFileData.mUsRaw->getSpacing()[1]; // convert to downwards movement in mm.
return hit;
}
int AudioCorrelation::addChild(AudioEnvelope *envelope, bool useFFT)
{
envelope->normalizeEnvelope();
const int sizeMain = m_mainTrackEnvelope->envelopeSize();
const int sizeSub = envelope->envelopeSize();
AudioCorrelationInfo *info = new AudioCorrelationInfo(sizeMain, sizeSub);
int64_t *correlation = info->correlationVector();
const int64_t *envMain = m_mainTrackEnvelope->envelope();
const int64_t *envSub = envelope->envelope();
int64_t max = 0;
if (useFFT) {
FFTCorrelation::correlate(envMain, sizeMain,
envSub, sizeSub,
correlation);
} else {
correlate(envMain, sizeMain,
envSub, sizeSub,
correlation,
&max);
info->setMax(max);
}
m_children.append(envelope);
m_correlations.append(info);
Q_ASSERT(m_correlations.size() == m_children.size());
return m_children.indexOf(envelope);
}
int main(unsigned long long id, unsigned long long argp, unsigned long long envp) {
id = id; // fix compiler warning
envp = envp; // fix compiler warning
init(argp);
decrementer_init();
// barrier();
unsigned int start = decrementer_get();
for(int i=0; i<ITERATIONS; i++) {
correlate();
}
unsigned int end = decrementer_get();
// barrier();
double seconds = decrementer_seconds(start - end);
unsigned long long ops = calc_ops() * ITERATIONS;
double gflops = (ops / seconds) / 1000000000.0;
double efficiency = (gflops / (SPU_FREQ * FLOPS_PER_CYCLE)) * 100.0;
unsigned long long loads = calc_loads() * ITERATIONS;
double gb = (double)loads / (1024.0*1024.0*1024.0);
double throughput = gb / seconds;
printf("SPU SECONDS = %f, ops = %lld, %6.3f gflops, efficiency is %6.3f %%, loads = %6.3f GB, %6.3f GB/s, efficiency is %6.3f %%\n",
seconds, ops, gflops, efficiency,
gb, throughput, throughput / TOTAL_HOST_MEM_BANDWIDTH * 100.0);
return 0;
}
bool generateMidamble(signalVector &gsmPulse,
int samplesPerSymbol,
int TSC)
{
if ((TSC < 0) || (TSC > 7))
return false;
if (gMidambles[TSC]) {
if (gMidambles[TSC]->sequence!=NULL) delete gMidambles[TSC]->sequence;
if (gMidambles[TSC]->sequenceReversedConjugated!=NULL) delete gMidambles[TSC]->sequenceReversedConjugated;
}
signalVector emptyPulse(1);
*(emptyPulse.begin()) = 1.0;
// only use middle 16 bits of each TSC
signalVector *middleMidamble = modulateBurst(gTrainingSequence[TSC].segment(5,16),
emptyPulse,
0,
samplesPerSymbol);
signalVector *midamble = modulateBurst(gTrainingSequence[TSC],
gsmPulse,
0,
samplesPerSymbol);
if (midamble == NULL) return false;
if (middleMidamble == NULL) return false;
// NOTE: Because ideal TSC 16-bit midamble is 66 symbols into burst,
// the ideal TSC has an + 180 degree phase shift,
// due to the pi/2 frequency shift, that
// needs to be accounted for.
// 26-midamble is 61 symbols into burst, has +90 degree phase shift.
scaleVector(*middleMidamble,complex(-1.0,0.0));
scaleVector(*midamble,complex(0.0,1.0));
signalVector *autocorr = correlate(midamble,middleMidamble,NULL,NO_DELAY);
if (autocorr == NULL) return false;
gMidambles[TSC] = new CorrelationSequence;
gMidambles[TSC]->sequence = middleMidamble;
gMidambles[TSC]->sequenceReversedConjugated = reverseConjugate(middleMidamble);
gMidambles[TSC]->gain = peakDetect(*autocorr,&gMidambles[TSC]->TOA,NULL);
LOG(DEBUG) << "midamble autocorr: " << *autocorr;
LOG(DEBUG) << "TOA: " << gMidambles[TSC]->TOA;
//gMidambles[TSC]->TOA -= 5*samplesPerSymbol;
delete autocorr;
delete midamble;
return true;
}
void DelayDetection::crossCorrelate(float *ref, float *seg, float *res, int refSize, int segSize)
{
// Output has same size as the
int rsize = refSize;
int ssize = segSize;
int tmpsize = segSize - refSize + 1;
// perform autocorrelation on reference signal
float acref = correlate(ref, ref, rsize);
// perform crossrelation on signal
float acseg = 0.0;
float r;
while (--tmpsize) {
--ssize;
acseg = correlate(seg+tmpsize, seg+tmpsize, rsize);
res[ssize] = correlate(ref, seg+tmpsize, rsize);
r = sqrt(acref*acseg);
if (r < 0.0000001)
res[ssize] = 0.0;
else
res[ssize] = res[ssize] / r;
}
// perform crosscorrelation on zerro padded region
int i = 0;
while (rsize) {
acseg = correlate(seg, seg, rsize);
res[ssize - 1] = correlate(ref + i, seg, rsize);
r = sqrt(acref * acseg);
if (r < 0.0001)
res[ssize - 1] = 0.0;
else
res[ssize - 1] = res[ssize-1] / r;
--rsize;
--ssize;
++i;
}
}
bool detectRACHBurst(signalVector &rxBurst,
float detectThreshold,
int samplesPerSymbol,
complex *amplitude,
float* TOA)
{
//static complex staticData[500];
//signalVector correlatedRACH(staticData,0,rxBurst.size());
signalVector correlatedRACH(rxBurst.size());
correlate(&rxBurst,gRACHSequence->sequenceReversedConjugated,&correlatedRACH,NO_DELAY,true);
float meanPower;
complex peakAmpl = peakDetect(correlatedRACH,TOA,&meanPower);
float valleyPower = 0.0;
// check for bogus results
if ((*TOA < 0.0) || (*TOA > correlatedRACH.size())) {
*amplitude = 0.0;
return false;
}
complex *peakPtr = correlatedRACH.begin() + (int) rint(*TOA);
LOG(DEBUG) << "RACH corr: " << correlatedRACH;
float numSamples = 0.0;
for (int i = 57*samplesPerSymbol; i <= 107*samplesPerSymbol;i++) {
if (peakPtr+i >= correlatedRACH.end())
break;
valleyPower += (peakPtr+i)->norm2();
numSamples++;
}
if (numSamples < 2) {
*amplitude = 0.0;
return false;
}
float RMS = sqrtf(valleyPower/(float) numSamples)+0.00001;
float peakToMean = peakAmpl.abs()/RMS;
LOG(DEBUG) << "RACH peakAmpl=" << peakAmpl << " RMS=" << RMS << " peakToMean=" << peakToMean;
*amplitude = peakAmpl/(gRACHSequence->gain);
*TOA = (*TOA) - gRACHSequence->TOA - 8*samplesPerSymbol;
LOG(DEBUG) << "RACH thresh: " << peakToMean;
return (peakToMean > detectThreshold);
}
void FFTCorrelation::correlate(const int64_t *left, const int leftSize,
const int64_t *right, const int rightSize,
int64_t *out_correlated)
{
float correlatedFloat[leftSize+rightSize+1];
correlate(left, leftSize, right, rightSize, correlatedFloat);
// The correlation vector will have entries up to N (number of entries
// of the vector), so converting to integers will not lose that much
// of precision.
for (int i = 0; i < leftSize+rightSize+1; i++) {
out_correlated[i] = correlatedFloat[i];
}
}
ViMainCorrelationWidget::ViMainCorrelationWidget(QWidget *parent)
: ViWidget(parent)
{
mUi = new Ui::ViMainCorrelationWidget();
mUi->setupUi(this);
mProject = NULL;
clear();
QString style = "QLabel { min-width: 80px; }";
mUi->projectLoader->setStyleSheet(style);
mUi->correlatorContainer->setStyleSheet(style);
mUi->projectLoader->setTypeMode(ViProjectLoader::NoTypes);
QObject::connect(mUi->projectLoader, SIGNAL(changed()), this, SLOT(correlate()));
}
vector<Point> findPoly(int vertices, vector<Point> points) {
double angle = (2 * M_PI) / ((double)vertices);
double dist = radius(points);
Point center = polyCenter(points);
Point offset = center + Point(0, 2 * dist);
vector<Point> result;
vector<Point> polygon;
Point first = rotateAround(center, offset, - (0.5 * angle));
polygon.push_back(first);
for (int i = 0; i < vertices - 1; i++) {
Point p = rotateAround(center, first, angle * (i+1));
polygon.push_back(p);
}
return correlate(polygon, points);
}
void CSelectMode::OnLeftDown( wxMouseEvent& event )
{
button_down_point = wxPoint(event.GetX(), event.GetY());
CurrentPoint = button_down_point;
m_button_down = true;
m_highlighted_objects.clear();
if(wxGetApp().m_dragging_moves_objects)
{
MarkedObjectManyOfSame marked_object;
wxGetApp().FindMarkedObject(button_down_point, &marked_object);
if(marked_object.m_map.size()>0)
{
HeeksObj* object = marked_object.GetFirstOfTopOnly();
if (event.ShiftDown())
{
// Augment the marked_object list with objects that 'look' like
// the one selected.
CCorrelationTool correlate(wxGetApp().m_min_correlation_factor, wxGetApp().m_max_scale_threshold, wxGetApp().m_number_of_sample_points, wxGetApp().m_correlate_by_color );
std::list<HeeksObj *> similar_objects = correlate.SimilarSymbols( object );
std::list<HeeksObj *>::const_iterator l_itSymbol;
for (l_itSymbol = similar_objects.begin(); l_itSymbol != similar_objects.end(); l_itSymbol++)
{
HeeksObj *ob = *l_itSymbol;
if (! wxGetApp().m_marked_list->ObjectMarked(ob))
{
wxGetApp().m_marked_list->Add(ob, true);
}
} // End for
} // End if - then
while(object)
{
if(object->GetType() == GripperType)
{
wxGetApp().m_current_viewport->DrawFront();
wxGetApp().drag_gripper = (Gripper*)object;
wxGetApp().m_digitizing->SetOnlyCoords(wxGetApp().drag_gripper, true);
wxGetApp().m_digitizing->digitize(button_down_point);
wxGetApp().grip_from = wxGetApp().m_digitizing->digitized_point.m_point;
wxGetApp().grip_to = wxGetApp().grip_from;
double from[3];
from[0] = wxGetApp().grip_from.X();
from[1] = wxGetApp().grip_from.Y();
from[2] = wxGetApp().grip_from.Z();
std::list<HeeksObj*> selected_objects;
for(std::list<HeeksObj*>::iterator It = wxGetApp().m_marked_list->list().begin(); It != wxGetApp().m_marked_list->list().end(); It++)
{
HeeksObj* object = *It;
if(object->CanBeDragged())selected_objects.push_back(object);
}
wxGetApp().drag_gripper->OnGripperGrabbed(selected_objects, true, from);
wxGetApp().grip_from = gp_Pnt(from[0], from[1], from[2]);
wxGetApp().m_current_viewport->EndDrawFront();
return;
}
object = marked_object.Increment();
}
}
}
}
int main(int argc, char **argv) {
FILE *IN, *OUT, *CMP;
dataset data;
field cmp;
int i, corr_func;
if (argc != 4 && argc != 5) {
printf( "correlator: arguments: outputfile datafile "
"comparison sample file [correlation method]\n"
"Supported correlation methods:\n"
"1 => Peebles-Hauser\n"
"2 => Davis-Peebles\n"
"3 => Hamilton\n"
"4 => Landy-Szalay (default)\n");
exit(EXIT_FAILURE);
}
/* Verify the correlation selection. */
if (argc == 5) {
corr_func = atoi(argv[4]);
if (corr_func <= 0 || corr_func > NUM_METHODS) {
fprintf(stderr, "Invalid correlation method %d\n",
corr_func);
exit(EXIT_FAILURE);
}
}
else
/* Prefer Landy-Szalay correlation function by default. */
corr_func = 4;
/* Open the file handles, exit on errors.*/
OUT = fopen(argv[1], "w");
if (OUT == NULL) {
perror("fopen");
exit(EXIT_FAILURE);
}
IN = fopen(argv[2], "r");
if (IN == NULL) {
perror("fopen");
exit(EXIT_FAILURE);
}
CMP = fopen(argv[3], "r");
if (CMP == NULL) {
perror("fopen");
exit(EXIT_FAILURE);
}
/* Read in the data. */
read_bins(IN, &data);
read_data(IN, &data.data);
read_data(CMP, &cmp);
/* Close input files. */
fclose(IN);
fclose(CMP);
/* Calculate and store the correlation. */
correlate(corr_func, &data, &cmp, OUT);
/* Final cleanup. */
fclose(OUT);
free(cmp.pts);
free(data.data.pts);
for (i=0; i < data.num_bins; i++) {
free(data.bins[i]);
}
free(data.bins);
return 0;
}
/*=============================================================================
* GET NBODY_2PCF
*=============================================================================*/
int get_nbody_2pcf(float *nbx, float *nby, float *nbz, float *nbm, long n_nb){
int ii,i,j;
long nend=0;
double *ercor;
unsigned long long *dd;
ercor = (double *) calloc(total_nr,sizeof(double));
dd = (unsigned long long *) calloc(total_nr,sizeof(unsigned long long));
// Find the boxsize of the nbody file.
float nbody_lbox = 0.0;
for (ii=0;ii<n_nb;ii++){
if(nbx[ii]>nbody_lbox) nbody_lbox = nbx[ii];
if(nby[ii]>nbody_lbox) nbody_lbox = nby[ii];
if(nbz[ii]>nbody_lbox) nbody_lbox = nbz[ii];
}
nbody_lbox = (float) ceil(nbody_lbox);
fprintf(stderr,"BOXSIZE OF NBODY: %e\n", nbody_lbox);
fprintf(stderr,"HALOS in NBODY: %ld\n",n_nb);
fprintf(stderr,"M[first]=%e, M[last]=%e \n",nbm[0],nbm[n_nb-1]);
// ALLOCATE the nbody_2pcf array
nbody_2pcf = (double **) calloc(Nalpha,sizeof(double *));
for(ii=0;ii<Nalpha;ii++){
// fprintf(stderr,"\ti_alpha=%d, mcuts= %e",ii,mcuts[ii]);
nbody_2pcf[ii] = (double *) calloc(total_nr,sizeof(double));
fprintf(stderr," allocated %d ",total_nr);
// fprintf(stderr,"\tNend=%ld\n",nend);
fprintf(stderr,"M[first]=%e \n",nbm[0]);
// fprintf(stderr,"M[last]=%e \n",nbm[n_nb-1]);
// fprintf(stderr,"\tnbm=%e\n",nbm[nend]);
#ifdef MASS_CUTS_FIT
while(nbm[nend]>mcuts[ii]){
nend++;
if(nend==n_nb){
fprintf(stderr,"ERROR: NBODY MASSES DON'T REACH MALPHA_MIN\n");
return -1;
}
// fprintf(stderr,"\n-%ld\n",nend);
}
#else
nend+=ncuts[ii];
#endif
fprintf(stderr,"\tNend=%ld nbm=%e\n",nend,nbm[nend]);
// Do the correlation
correlate(nend, nbody_lbox,nbx,nby,nbz,nbody_2pcf[ii],ercor,dd,total_nr,maxr,160);
fprintf(stderr,"\tCorrelated\n\n");
}
// OUTPUT
FILE* tpcfile;
tpcfile = fopen(OutputNbody2PCF,"w");
if(tpcfile==NULL){
fprintf(stderr,"UNABLE TO OPEN NBODY 2PCF FILE\n");
exit(1);
}
for(i=0;i<total_nr;i++){
for(j=0;j<Nalpha;j++){
fprintf(tpcfile,"%e\t",nbody_2pcf[j][i]);
}
fprintf(tpcfile,"\n");
}
fclose(tpcfile);
fprintf(stderr,"NBODY FILE WRITTEN\n");
return 0;
}
int main(int argc, char **argv){
fprintf(stderr,"\n*******************************************************************\n");
fprintf(stderr,"** **\n");
fprintf(stderr,"** = HALOGEN FIT V0.5 = **\n");
fprintf(stderr,"** **\n");
fprintf(stderr,"** **\n");
fprintf(stderr,"** let there be knowledge of the dark **\n");
fprintf(stderr,"*******************************************************************\n\n");
if (argc!=2){
fprintf(stderr,"usage: %s inputfile\n",argv[0]);
return -1;
}
char inname[256];
float *hx, *hy, *hz, *hvx, *hvy, *hvz, *hR;
strcpy(inname,argv[1]);
double dr;
double **halogen_2pcf, **halogen_err;
long nend;
unsigned long long *dd;
long i,j,ii;
#ifdef VERB
fprintf(stderr,"#def VERB\n");
#endif
#ifdef DEBUG
fprintf(stderr,"#def DEBUG \n");
#endif
#ifdef ULTRADEBUG
fprintf(stderr,"#def ULTRADEBUG \n");
#endif
#ifdef ONLYBIG
fprintf(stderr,"#def ONLYBIG\n");
#endif
#ifdef NO_EXCLUSION
fprintf(stderr,"#def NO_EXCLUSION\n");
#endif
#ifdef NO_MASS_CONSERVATION
fprintf(stderr,"#def NO_MASS_CONSERVATION\n");
#endif
#ifdef MASS_OF_PARTS
fprintf(stderr,"#def MASS_OF_PARTS\n");
#endif
#ifdef RANKED
fprintf(stderr,"#def RANKED\n");
#endif
#ifdef NDENS
fprintf(stderr,"#def NDENS\n");
#endif
#ifdef NO_PROB_PARALEL
fprintf(stderr,"#def NO_PROB_PARALEL\n");
#endif
#ifdef NO_PAR_FIT
fprintf(stderr,"#def NO_PAR_FIT\n");
#endif
#ifdef MASS_CUTS_FIT
fprintf(stderr,"#def MASS_CUTS_FIT\n");
#endif
#ifdef BETA0
fprintf(stderr,"#def BETA0\n");
#endif
fprintf(stderr,"\nReading input file...\n");
if (read_input_file(inname)<0)
return -1;
fprintf(stderr,"... file read correctly!\n");
//SETUP OUTPUT FILES
strcpy(OutputNbody2PCF,OutputDir);
strcpy(OutputHalogen2PCF,OutputDir);
strcpy(OutputHalogenErr,OutputDir);
strcpy(OutputAlphaM,OutputDir);
strcpy(OutputExampleCat,OutputDir);
strcat(OutputNbody2PCF,"/nbody.2pcf");
strcat(OutputHalogen2PCF,"/halogen.2pcf");
strcat(OutputHalogenErr,"/halogen.err");
strcat(OutputAlphaM,"/M-alpha.txt");
strcat(OutputExampleCat,"/example.halos");
fprintf(stderr,"Reading Gadget file(s)...\n");
if (read_snapshot(Snapshot, format, LUNIT, MUNIT, SWP, LGADGET, DGADGET,Nlin,&x, &y, &z, &vx, &vy, &vz, &Npart, &mpart, &Lbox, &om_m,&ListOfParticles,&NPartPerCell)==0)
fprintf(stderr,"...Gadget file(s) correctly read!\n");
else {
fprintf(stderr,"error: Something went wrong reading the gadget file %s\n",inname);
return -1;
}
#ifdef VERB
fprintf(stderr,"\n\tCheck: Npart=%ld, mpart=%e, Lbox=%f\n",Npart,mpart,Lbox);
fprintf(stderr,"\tx[0]= %f, y[0]= %f, z[0]= %f\n",(x)[0],(y)[0],(z)[0]);
fprintf(stderr,"\t\tvx[0]= %f, vy[0]= %f, vz[0]= %f\n",(vx)[0],(vy)[0],(vz)[0]);
fprintf(stderr,"\t ...\n");
fprintf(stderr,"\tx[%ld]= %f, y[%ld]= %f, z[%ld]= %f\n",Npart-1,(x)[Npart-1],Npart-1,(y)[Npart-1],Npart-1,(z)[Npart-1]);
fprintf(stderr,"\t\tvx[%ld]= %f, vy[%ld]= %f, vz[%ld]= %f\n\n",Npart-1,(vx)[Npart-1],Npart-1,(vy)[Npart-1],Npart-1,(vz)[Npart-1]);
#endif
seed = time(NULL);
//Generate the halo masses from the mass function
fprintf(stderr,"Generating Halo Masses...\n");
Nhalos = populate_mass_function(MassFunctionFile,Mmin,Lbox,&HaloMass,seed,160);
if (Nhalos<0){
fprintf(stderr,"error: Couldnt create HaloMass array\n");
return -1;
}
fprintf(stderr,"...Halo Masses Generated\n");
// GET THE ACTUAL NUMBER OF R VALUES IN CORR (LINEARLY SPACED)
dr = (maxr - minr) / nr;
total_nr = (int) ceil(maxr / dr) + 2;
maxr += 2* dr;
//density at the boundary of a halo
if (strcmp(rho_ref,"crit")==0)
rho = OVD*rho_crit;
else
rho = OVD*rho_crit*om_m;
Nmin = Lbox*Lbox*Lbox*Dmin;
Nmax = Lbox*Lbox*Lbox*Dmax;
#ifdef VERB
fprintf(stderr,"Generating Mass Bins...\n");
#endif
Nalpha = get_mass_bins(HaloMass,Nhalos,Nmax,Nmin);
#ifdef VERB
fprintf(stderr,"... generated mass bins\n");
fprintf(stderr,"Nmin=%d, Nmax=%d, Nmassbins=%d (=Nalphas).\n",Nmin,Nmax,Nalpha);
fprintf(stderr,"Getting NBODY 2PCF...\n");
#endif
float *nbx, *nby, *nbz, *nbvx, *nbvy, *nbvz, *nbm,*fvel;
long nb_n;
// Import the Nbody halos
nb_n = read_nbody(&nbx,&nby,&nbz,&nbvx,&nbvy,&nbvz,&nbm);
fprintf(stderr,"READ IT IN\n");
// Get the Nbody 2PCF
get_nbody_2pcf(nbx,nby,nbz,nbm,nb_n);
free(nbx);
free(nby);
free(nbz);
free(nbm);
betavec = (double *)calloc(Nalpha,sizeof(double));
alphavec = (double *)calloc(Nalpha,sizeof(double));
for (i=0;i<Nalpha;i++){
betavec[i]=1.0;
}
// DO THE FIT
if(find_best_alpha()==0)
fprintf(stderr, "... done fitting.\n");
else {
fprintf(stderr,"Problem fitting\n");
return -1;
}
hx = (float *) calloc(Nhalos,sizeof(float));
hy = (float *) calloc(Nhalos,sizeof(float));
hz = (float *) calloc(Nhalos,sizeof(float));
hvx = (float *) calloc(Nhalos,sizeof(float));
hvy = (float *) calloc(Nhalos,sizeof(float));
hvz = (float *) calloc(Nhalos,sizeof(float));
hR = (float *) calloc(Nhalos,sizeof(float));
seed++;
// Do a final placement with the correct alphavec
place_halos(Nhalos,HaloMass, Nlin, Npart, x, y, z, vx,vy,vz,Lbox,
rho,seed,
mpart,nthreads, alphavec, betavec, mcuts, Nalpha, recalc_frac,hx, hy, hz,
hvx,hvy,hvz, hR,ListOfParticles,NPartPerCell);
fprintf(stderr,"\tComputing velocity bias fvel...\n");
fvel = compute_fvel(hvx,hvy,hvz,nbvx,nbvy,nbvz,nbm);
free(nbvx);
free(nbvy);
free(nbvz);
#ifdef VERB
for (i=0;i<Nalpha;i++){
fprintf(stderr,"alpha[%ld]=%f beta[%ld]=%f fvel=%f\n",i,alphavec[i],i,betavec[i], fvel[i]);
}
#endif
fprintf(stderr,"\t...done!\n");
// ALLOCATE the halogen_2pcf array
halogen_2pcf = (double **) calloc(Nalpha,sizeof(double *));
halogen_err = (double **) calloc(Nalpha,sizeof(double *));
nend = 0;
dd = (unsigned long long *) calloc(total_nr,sizeof(unsigned long long));
for(ii=0;ii<Nalpha;ii++){
(halogen_2pcf)[ii] = (double *) calloc(total_nr,sizeof(double));
(halogen_err)[ii] = (double *) calloc(total_nr,sizeof(double));
while(HaloMass[nend]>mcuts[ii] && nend<Nhalos){
nend++;
}
if(nend==Nhalos){
fprintf(stderr,"ERROR: HALOMASSES DON'T REACH MALPHA_MIN\n");
return -1;
}
// Do the correlation
correlate(nend, Lbox,hx,hy,hz,halogen_2pcf[ii],halogen_err[ii], dd,total_nr,maxr,160);
}
// OUTPUT
FILE* tpcfile;
FILE* errfile;
tpcfile = fopen(OutputHalogen2PCF,"w");
errfile = fopen(OutputHalogenErr,"w");
if(tpcfile==NULL){
fprintf(stderr,"UNABLE TO OPEN NBODY 2PCF FILE\n");
exit(1);
}
if(errfile==NULL){
fprintf(stderr,"UNABLE TO OPEN NBODY 2PCF FILE\n");
exit(1);
}
fprintf(stderr,"OPENED TPC AND ERR\n");
for(i=0;i<total_nr;i++){
fprintf(tpcfile,"%e\t",(i+0.5)*dr);
fprintf(errfile,"%e\t",(i+0.5)*dr);
for(j=0;j<Nalpha;j++){
fprintf(tpcfile,"%e\t",halogen_2pcf[j][i]);
fprintf(errfile,"%e\t",halogen_err[j][i]);
}
fprintf(stderr,"\n");
fprintf(tpcfile,"\n");
fprintf(errfile,"\n");
}
fclose(tpcfile);
fclose(errfile);
fprintf(stderr,"outputted tpc and errfile\n");
FILE* alphafile;
alphafile = fopen(OutputAlphaM,"w");
for(i=0;i<Nalpha;i++){
fprintf(alphafile,"%e\t%e\t%e\n",mcuts[i],alphavec[i],fvel[i]);
}
fclose(alphafile);
for(ii=0;ii<Nalpha;ii++){
free((halogen_2pcf)[ii]);
free((halogen_err)[ii]);
}
// TBD: change this for a loop correcting velocities or change it on the fly while writing
place_halos(Nhalos,HaloMass, Nlin, Npart, x, y, z, vx,vy,vz,Lbox,
rho,seed,
mpart,nthreads, alphavec, betavec, mcuts, Nalpha, recalc_frac,hx, hy, hz,
hvx,hvy,hvz, hR,ListOfParticles,NPartPerCell);
fprintf(stderr,"Writing an example halo catalog to %s\n",OutputExampleCat);
write_halogen_cat(OutputExampleCat, hx, hy, hz, hvx, hvy,hvz, HaloMass, hR,Nhalos,fvel,mcuts);
fprintf(stderr,"...done!\n");
free(halogen_2pcf);
free(halogen_err);
free(dd);
free(hvx);
free(hvy);
free(hvz);
free(hx);
free(hy);
free(hz);
free(hR);
fprintf(stderr,"\n*******************************************************************\n");
fprintf(stderr,"** ... the fit is complete. **\n");
fprintf(stderr,"*******************************************************************\n\n");
return 0;
}
bool analyzeTrafficBurst(signalVector &rxBurst,
unsigned TSC,
float detectThreshold,
int samplesPerSymbol,
complex *amplitude,
float *TOA,
unsigned maxTOA,
bool requestChannel,
signalVector **channelResponse,
float *channelResponseOffset)
{
assert(TSC<8);
assert(amplitude);
assert(TOA);
assert(gMidambles[TSC]);
if (maxTOA < 3*samplesPerSymbol) maxTOA = 3*samplesPerSymbol;
unsigned spanTOA = maxTOA;
if (spanTOA < 5*samplesPerSymbol) spanTOA = 5*samplesPerSymbol;
unsigned startIx = (66-spanTOA)*samplesPerSymbol;
unsigned endIx = (66+16+spanTOA)*samplesPerSymbol;
unsigned windowLen = endIx - startIx;
unsigned corrLen = 2*maxTOA+1;
unsigned expectedTOAPeak = (unsigned) round(gMidambles[TSC]->TOA + (gMidambles[TSC]->sequenceReversedConjugated->size()-1)/2);
signalVector burstSegment(rxBurst.begin(),startIx,windowLen);
static complex staticData[200];
signalVector correlatedBurst(staticData,0,corrLen);
correlate(&burstSegment, gMidambles[TSC]->sequenceReversedConjugated,
&correlatedBurst, CUSTOM,true,
expectedTOAPeak-maxTOA,corrLen);
float meanPower;
*amplitude = peakDetect(correlatedBurst,TOA,&meanPower);
float valleyPower = 0.0; //amplitude->norm2();
complex *peakPtr = correlatedBurst.begin() + (int) rint(*TOA);
// check for bogus results
if ((*TOA < 0.0) || (*TOA > correlatedBurst.size())) {
*amplitude = 0.0;
return false;
}
int numRms = 0;
for (int i = 2*samplesPerSymbol; i <= 5*samplesPerSymbol;i++) {
if (peakPtr - i >= correlatedBurst.begin()) {
valleyPower += (peakPtr-i)->norm2();
numRms++;
}
if (peakPtr + i < correlatedBurst.end()) {
valleyPower += (peakPtr+i)->norm2();
numRms++;
}
}
if (numRms < 2) {
// check for bogus results
*amplitude = 0.0;
return false;
}
float RMS = sqrtf(valleyPower/(float)numRms)+0.00001;
float peakToMean = (amplitude->abs())/RMS;
// NOTE: Because ideal TSC is 66 symbols into burst,
// the ideal TSC has an +/- 180 degree phase shift,
// due to the pi/4 frequency shift, that
// needs to be accounted for.
*amplitude = (*amplitude)/gMidambles[TSC]->gain;
*TOA = (*TOA) - (maxTOA);
LOG(DEBUG) << "TCH peakAmpl=" << amplitude->abs() << " RMS=" << RMS << " peakToMean=" << peakToMean << " TOA=" << *TOA;
LOG(DEBUG) << "autocorr: " << correlatedBurst;
if (requestChannel && (peakToMean > detectThreshold)) {
float TOAoffset = maxTOA; //gMidambles[TSC]->TOA+(66*samplesPerSymbol-startIx);
delayVector(correlatedBurst,-(*TOA));
// midamble only allows estimation of a 6-tap channel
signalVector channelVector(6*samplesPerSymbol);
float maxEnergy = -1.0;
int maxI = -1;
for (int i = 0; i < 7; i++) {
if (TOAoffset+(i-5)*samplesPerSymbol + channelVector.size() > correlatedBurst.size()) continue;
if (TOAoffset+(i-5)*samplesPerSymbol < 0) continue;
correlatedBurst.segmentCopyTo(channelVector,(int) floor(TOAoffset+(i-5)*samplesPerSymbol),channelVector.size());
float energy = vectorNorm2(channelVector);
if (energy > 0.95*maxEnergy) {
maxI = i;
maxEnergy = energy;
}
}
*channelResponse = new signalVector(channelVector.size());
correlatedBurst.segmentCopyTo(**channelResponse,(int) floor(TOAoffset+(maxI-5)*samplesPerSymbol),(*channelResponse)->size());
scaleVector(**channelResponse,complex(1.0,0.0)/gMidambles[TSC]->gain);
LOG(DEEPDEBUG) << "channelResponse: " << **channelResponse;
if (channelResponseOffset)
*channelResponseOffset = 5*samplesPerSymbol-maxI;
}
return (peakToMean > detectThreshold);
}
Point RefreshingTracker::correlate(const ConstImageAdapter<double>& adapter) {
PhaseCorrelator pc;
return correlate(adapter, pc);
}
void ImFCSCalibrationWizard::on_btnCorrelate_clicked()
{
emit correlate();
}
/*=============================================================================
* FITTING CODE
*=============================================================================*/
int find_best_alpha(){
//double alpha_2pcf[num_alpha][nr], trials_2pcf[num_alpha][ntrials][total_nr], alpha_err[num_alpha][nr];
double **alpha_2pcf, ***trials_2pcf, **alpha_err;
double low, high, da;
long i,nend,k,j,ir,jk;
double chi2;
double yi,yerr,xi,minerr;
int nbreak, iii,ind,min_ind,max_ind;
long thisseed[Nalpha*num_alpha*ntrials];
#ifdef ALLOUT
char Output2PCF[LINELENGTH];
char ThisFile[15];
#endif
ncoeffs = num_alpha-1;
nbreak =ncoeffs-2;
// Allocate alphavec, which is our result
//alphavec = (double *) calloc(Nalpha,sizeof(double));
//betavec = (double *) calloc(Nalpha,sizeof(double));
alpha_2pcf = (double **) calloc(num_alpha,sizeof(double *));
alpha_err = (double **) calloc(num_alpha,sizeof(double *));
trials_2pcf = (double ***) calloc(num_alpha,sizeof(double **));
for (i=0;i<num_alpha;i++){
alpha_2pcf[i] = (double *) calloc(nr,sizeof(double));
alpha_err[i] = (double *) calloc(nr,sizeof(double));
trials_2pcf[i] = (double **) calloc(ntrials,sizeof(double*));
for (j=0;j<ntrials;j++)
trials_2pcf[i][j] = (double *) calloc(total_nr,sizeof(double));
}
// allocate a cubic bspline workspace (k = 4)
bw = gsl_bspline_alloc(4, nbreak);
B = gsl_vector_alloc(ncoeffs);
alpha_grid = gsl_vector_alloc(num_alpha);
chi2_alpha = gsl_vector_alloc(num_alpha);
X = gsl_matrix_alloc(num_alpha,ncoeffs);
c = gsl_vector_alloc(ncoeffs);
weights = gsl_vector_alloc(num_alpha);
cov = gsl_matrix_alloc(ncoeffs, ncoeffs);
mw = gsl_multifit_linear_alloc(num_alpha, ncoeffs);
// Loop through each mass bin
nend = 0;
iii = 0;
seed = time(NULL);
for (i=0;i<Nalpha;i++){
for(j=0;j<num_alpha;j++){
for(k=0;k<ntrials;k++){
thisseed[iii] = seed + iii;
iii++;
}
}
}
fprintf(stderr,"STARTING LOOPS...\n");
iii = 0;
for (i=0;i<Nalpha;i++){
// Get where to place halos to
nend += ncuts[i];
// Calculate a more optimum alpha_grid for this bin
if (i < 2)
low = alpha_ratio_1*best_alpha;
else{
low = alpha_ratio*best_alpha;
}
high = 1.05*best_alpha;
da = (high-low)/(num_alpha-1);
fprintf(stderr,"STARTING THREADS\n");
#ifndef NO_PAR_FIT
#pragma omp parallel for num_threads(nthreads) private(jk,j,k) \
shared(num_alpha,ntrials,i,alpha_grid,low,da,stderr,Nalpha,alphavec,iii,mcuts,ListOfParticles,\
NPartPerCell,x,y,z,Lbox,Npart,Nlin,HaloMass,nend,mpart,recalc_frac,betavec,thisseed,trials_2pcf,rho,maxr,\
total_nr,Nhalos) default(none)
#endif
for(jk=0;jk<num_alpha*ntrials;jk++){
fprintf(stderr,"Thread %d/%d\n",omp_get_thread_num(),omp_get_num_threads());
float *hx,*hy,*hz,*hR;
double *thisalphavec;
double *ercorr;
unsigned long long *DD;
hx = (float *) calloc(Nhalos,sizeof(float));
hy = (float *) calloc(Nhalos,sizeof(float));
hz = (float *) calloc(Nhalos,sizeof(float));
hR = (float *) calloc(Nhalos,sizeof(float));
thisalphavec = (double *) calloc(Nalpha,sizeof(double));
DD = (unsigned long long *) calloc(total_nr,sizeof(unsigned long long));
ercorr = (double *) calloc(total_nr,sizeof(double));
k=jk%ntrials;
j=jk/ntrials;
fprintf(stderr,"MOVED MEMORY\n");
fprintf(stderr,"GOT k,j: %ld, %ld\n",k,j);
fprintf(stderr,"sizeof M : %f MB\n",(Nlin*Nlin*Nlin*sizeof(double)/1024./1024.));
//define the alpha grid for this mass bin
gsl_vector_set(alpha_grid,j,low+j*da);
// create a local alphavec, to which we add the current gridded alpha
int itemp;
for (itemp=0;itemp<Nalpha;itemp++)
thisalphavec[itemp] = alphavec[itemp];
//fprintf(stderr,"mem copied\n");
thisalphavec[i] = gsl_vector_get(alpha_grid,j);
// Place the halos
#ifdef NO_PAR_FIT
place_halos(nend,HaloMass, Nlin, Npart, x, y, z, NULL,NULL,NULL,Lbox,
rho,thisseed[jk+i*num_alpha*ntrials],
mpart,nthreads,thisalphavec, betavec, mcuts, Nalpha, recalc_frac,hx, hy, hz,
NULL,NULL,NULL,hR,ListOfParticles,NPartPerCell);
#else
place_halos(nend,HaloMass, Nlin, Npart, x, y, z, NULL,NULL,NULL,Lbox,
rho,thisseed[jk+i*num_alpha*ntrials],
mpart,1,thisalphavec, betavec, mcuts, Nalpha, recalc_frac,hx, hy, hz,
NULL,NULL,NULL,hR,ListOfParticles,NPartPerCell);
#endif
fprintf(stderr,"correlating...\n");
//Get the 2PCF
correlate(nend, Lbox,hx,hy,hz,trials_2pcf[j][k], ercorr, DD,total_nr,maxr,1);
fprintf(stderr,"...correlated\n");
free(hx);
free(hy);
free(hz);
free(hR);
free(thisalphavec);
free(ercorr);
free(DD);
}
for (jk=0;jk<num_alpha*ntrials;jk++){
k=jk%ntrials;
j=jk/ntrials;
fprintf(stderr,"RAWCORR %ld, %ld: %e\n",j,k,trials_2pcf[j][k][0]);
}
// #pragma omp parallel for private(ir,j,chi2,k) shared(num_alpha,stderr,i,nr,alpha_2pcf) default(none)
fprintf(stderr,"GOT CORRELATIONS , GETTING CHI2...\n");
for(j=0;j<num_alpha;j++){
//Get mean and stdev of trials_2pcf
chi2 = 0.0;
for (ir=0;ir<nr;ir++){
alpha_2pcf[j][ir] = 0.0;
for(k=0;k<ntrials;k++){
alpha_2pcf[j][ir] += trials_2pcf[j][k][ir+total_nr-nr-2];
}
alpha_2pcf[j][ir] = alpha_2pcf[j][ir]/ntrials;
alpha_err[j][ir] = 0.0;
for(k=0;k<ntrials;k++){
alpha_err[j][ir] += pow((trials_2pcf[j][k][ir+total_nr-nr-2]-alpha_2pcf[j][ir]),2);
}
alpha_err[j][ir] = pow((alpha_err[j][ir]/(ntrials-1)),0.5);
// Now get chi^2 values
#ifdef REL_CHI2
chi2 += pow(((alpha_2pcf[j][ir]-nbody_2pcf[i][ir+total_nr-nr-2])/nbody_2pcf[i][ir+total_nr-nr-2]),2);
#else
chi2 += pow(((alpha_2pcf[j][ir]-nbody_2pcf[i][ir+total_nr-nr-2])/alpha_err[j][ir]),2);
#endif
fprintf(stderr,"%ld, %ld, %e, %e, %e, %e\n",j,ir,alpha_2pcf[j][ir],nbody_2pcf[i][ir+total_nr-nr-2],alpha_err[j][ir],chi2);
}
gsl_vector_set(chi2_alpha,j,chi2/nr);
gsl_vector_set(weights,j,nr/chi2);
}
// #endif //NO_PARALLEL_FIT
//*/
#ifdef ALLOUT
//OUTPUT SOME THINGS FOR CHECKING
sprintf(ThisFile,"/raw.2pcf.%ld",i);
strcpy(Output2PCF,OutputDir);
strcat(Output2PCF,ThisFile);
FILE* output_2pcf;
output_2pcf = fopen(Output2PCF,"w");
//header
fprintf(output_2pcf,"# r, ");
for (k=0;k<num_alpha;k++){
fprintf(output_2pcf,"%e\t",gsl_vector_get(alpha_grid,k));
}
fprintf(output_2pcf,"\n");
//table
for (ir=0;ir<nr;ir++){
fprintf(output_2pcf,"%e\t",(ir+total_nr-nr+0.5)*maxr/total_nr);
for(k=0;k<num_alpha-1;k++){
fprintf(output_2pcf,"%e\t",alpha_2pcf[k][ir]);
}
fprintf(output_2pcf,"%e\n",alpha_2pcf[num_alpha-1][ir]);
}
fclose(output_2pcf);
sprintf(ThisFile,"/raw.err.%ld",i);
strcpy(Output2PCF,OutputDir);
strcat(Output2PCF,ThisFile);
FILE* output_err;
output_err = fopen(Output2PCF,"w");
//header
fprintf(output_err,"# r, ");
for (k=0;k<num_alpha;k++){
fprintf(output_err,"%e\t",gsl_vector_get(alpha_grid,k));
}
fprintf(output_err,"\n");
//table
for (ir=0;ir<nr;ir++){
fprintf(output_err,"%e\t",(ir+total_nr-nr+0.5)*maxr/total_nr);
for(k=0;k<num_alpha-1;k++){
fprintf(output_err,"%e\t",alpha_err[k][ir]);
}
fprintf(output_err,"%e\n",alpha_err[num_alpha-1][ir]);
}
fclose(output_err);
sprintf(ThisFile,"/chi2.%ld",i);
strcpy(Output2PCF,OutputDir);
strcat(Output2PCF,ThisFile);
FILE* chifile;
chifile = fopen(Output2PCF,"w");
fprintf(chifile,"# alpha, chi2, weight\n");
for (k=0;k<num_alpha;k++){
fprintf(chifile,"%e\t%e\t%e\n",gsl_vector_get(alpha_grid,k),gsl_vector_get(chi2_alpha,k),
gsl_vector_get(weights,k));
}
fclose(chifile);
#endif
// Check if final value or initial value is the smallest
minerr = gsl_vector_get(chi2_alpha,0);
ind = 0;
for(k=1;k<num_alpha;k++){
if(minerr>gsl_vector_get(chi2_alpha,k)){
minerr = gsl_vector_get(chi2_alpha,k);
ind = k;
}
}
if(ind==0){
fprintf(stderr,"ERROR: alpha_grid doesn't extend low enough, set alpha_ratio lower");
}
if(ind==num_alpha-1){
fprintf(stderr,"ERROR: alpha_grid doesn't extend high enough, set best_alpha higher");
}
if (ind>=2){
min_ind = ind-2;
}else{
min_ind = 0;
}
if (ind<=num_alpha-3){
max_ind = ind+2;
}else{
max_ind = num_alpha-1;
}
/* use uniform breakpoints on interval */
gsl_bspline_knots_uniform(gsl_vector_get(alpha_grid,0), gsl_vector_get(alpha_grid,num_alpha-1), bw);
/* construct the fit matrix X */
for (k = 0; k < num_alpha; ++k){
double xi = gsl_vector_get(alpha_grid, k);
/* compute B_j(xi) for all j */
gsl_bspline_eval(xi, B, bw);
/* fill in row i of X */
for (j = 0; j < ncoeffs; ++j){
double Bj = gsl_vector_get(B, j);
gsl_matrix_set(X, k, j, Bj);
}
}
fprintf(stderr,"Got to the fit part\n");
/* do the fit */
gsl_multifit_wlinear(X, weights, chi2_alpha, c, cov, &chisq, mw);
// PRINT OUT EVERYTHING WE HAVE SO FAR
fprintf(stderr,"alpha\tchi2_alpha\t\n");
for (k=0;k<num_alpha;k++){
fprintf(stderr,"%e\t%e\t\n",gsl_vector_get(alpha_grid,k),
gsl_vector_get(chi2_alpha,k));
}
#ifdef VERB
dof = num_alpha - ncoeffs;
tss = gsl_stats_wtss(weights->data, 1, chi2_alpha->data, 1, chi2_alpha->size);
Rsq = 1.0 - chisq / tss;
fprintf(stderr, "chisq/dof = %e, Rsq = %f\n",chisq / dof, Rsq);
#endif
for (xi=gsl_vector_get(alpha_grid,0);xi<gsl_vector_get(alpha_grid,num_alpha-1);xi+=0.01){
gsl_bspline_eval(xi, B, bw);
gsl_multifit_linear_est(B, c, cov, &yi, &yerr);
fprintf(stderr,"%e\t%e\t%e\n",xi,yi,gsl_vector_get(B,0));
}
//DO THE MINIMIZATION
alphavec[i] = minimize(gsl_vector_get(alpha_grid,min_ind), gsl_vector_get(alpha_grid,max_ind),
gsl_vector_get(alpha_grid,ind));
best_alpha = alphavec[i];
fprintf(stderr,"\n Best alpha: %f\n\n",best_alpha);
}
return 0;
}
univector<T> autocorrelate(const univector_ref<const T>& src1)
{
univector<T> result = correlate(src1, src1);
result = result.slice(result.size() / 2);
return result;
}
int *findPossibleBlinks( int *num_blinks, const NUMTYPE *channel,
int len_channel, const BlinkParams *params )
{
unsigned int i, j, max_level, len_coef, lengths[8];
int cor_length, temp_start, len_template;
NUMTYPE *coefs, *chan_a, *chan_d, *chan_r, *thresh, *templ;
NUMTYPE min;
int above, min_index, num_indices, *indices, steps_1, index, *blinks;
NUMTYPE *cors;
// Pull out the parameters that we'll be using.
NUMTYPE t_1 = params->t_1;
NUMTYPE t_cor = params->t_cor;
// Make sure that we are capable of taking the 5th, 6th, and 7th level
// decompositions.
max_level = wavedecMaxLevel( len_channel, COIF3.len_filter );
if (max_level < 7) {
fprintf( stderr, "Channel too short for processing!\n" );
(*num_blinks) = 0;
return NULL;
}
//////////////////////////////////////////////////////////////////////////////
// Allocate memory.
//////////////////////////////////////////////////////////////////////////////
len_coef = wavedecResultLength( len_channel, COIF3.len_filter, 7 );
coefs = (NUMTYPE*) malloc( sizeof(NUMTYPE) * len_coef );
chan_a = (NUMTYPE*) malloc( sizeof(NUMTYPE) * len_channel );
chan_d = (NUMTYPE*) malloc( sizeof(NUMTYPE) * len_channel );
chan_r = (NUMTYPE*) malloc( sizeof(NUMTYPE) * len_channel );
thresh = (NUMTYPE*) malloc( sizeof(NUMTYPE) * len_channel );
//////////////////////////////////////////////////////////////////////////////
// Extract the appropriate approximation and detail signals.
//////////////////////////////////////////////////////////////////////////////
wavedec( coefs, lengths, channel, len_channel, COIF3, 7 );
wrcoef( chan_a, coefs, lengths, 8, RECON_APPROX, COIF3, 3 );
// Sum the enveloped 5th, 6th, and 7th level details.
for (i = 0; i < len_channel; i++) { chan_d[i] = 0.0; }
for (i = 5; i <= 7; i++) {
wrcoef( chan_r, coefs, lengths, 8, RECON_DETAIL, COIF3, i );
envelope( chan_r, len_channel );
for (j = 0; j < len_channel; j++) {
chan_d[j] += chan_r[j];
}
}
//////////////////////////////////////////////////////////////////////////////
// Generate the first guess at eyeblink locations.
//////////////////////////////////////////////////////////////////////////////
// Compute the moving average threshold.
movingAverage( thresh, chan_d, len_channel );
for (i = 0; i < len_channel; i++) { thresh[i] += t_1; }
// Find the minimums of the original channel between rising/falling edge pairs
// of the enveloped channel details.
above = 0; num_indices = 0; min = INFINITY; min_index = 0; indices = NULL;
for (i = 1; i < len_channel; i++) {
// Check to see if we crossed the threshold.
if (!above && chan_d[i] > thresh[i] && chan_d[i-1] <= thresh[i-1]) {
above = 1;
// Reset the 'min' value so that we can find a new minimum.
min = INFINITY;
} else if (above && chan_d[i] < thresh[i] && chan_d[i-1] >= thresh[i-1]) {
above = 0;
// We just found the falling edge of a pair. Record the index of the
// minimum value that we found within the pair.
num_indices++;
indices = (int*) realloc( indices, sizeof(int) * num_indices );
indices[num_indices-1] = min_index;
}
// If we're above the threshold, keep track of the minimum value that we
// see.
if (above) {
if (min > chan_a[i]) {
min = chan_a[i];
min_index = i;
}
}
}
//////////////////////////////////////////////////////////////////////////////
// Refine our first guess, getting our final guess at eyeblink locations.
//////////////////////////////////////////////////////////////////////////////
cors = (NUMTYPE*) malloc( sizeof(NUMTYPE) * num_indices );
// Generate the eyeblink template to correlate with the eyeblink locations.
templ = getTemplate( &steps_1, &len_template, params );
// Calculate the correlations of the original channel around the minimum
// points.
(*num_blinks) = num_indices;
for (i = 0; i < num_indices; i++) {
index = indices[i] - steps_1; temp_start = 0; cor_length = len_template;
// Make sure the correlation function will not access beyond the bounds
// of our arrays.
if (index < 0) {
temp_start = -index;
cor_length = len_template + index;
index = 0;
} else if (index + len_template >= len_channel) {
cor_length = len_template - (index + len_template - len_channel);
}
cors[i] = correlate( chan_a + index, templ + temp_start, cor_length );
// Mark for elimination the indices with correlations below the threshold.
if (cors[i] < t_cor) {
indices[i] = -1;
(*num_blinks)--;
}
}
// If we still have blinks after all this, allocate memory for them.
if ((*num_blinks) == 0) {
blinks = NULL;
} else {
blinks = (int*) malloc( sizeof(int) * (*num_blinks) );
index = 0;
for (i = 0; i < num_indices; i++) {
if (indices[i] > 0) {
blinks[index] = indices[i];
index++;
}
}
}
//////////////////////////////////////////////////////////////////////////////
// Clean up and return.
//////////////////////////////////////////////////////////////////////////////
free( indices ); free( cors ); free( coefs );
free( chan_a ); free( chan_d ); free( chan_r );
free( thresh ); free( templ );
return blinks;
}
Contour Contour::interCorrelate(const Contour& c) const {
return correlate(c, 0);
}
void CSelectMode::OnMouse( wxMouseEvent& event )
{
bool event_used = false;
if(LeftAndRightPressed(event, event_used))
{
if(m_doing_a_main_loop){
wxGetApp().ExitMainLoop();
}
}
if(event_used)return;
if(event.LeftDown())
{
button_down_point = wxPoint(event.GetX(), event.GetY());
CurrentPoint = button_down_point;
if(wxGetApp().m_dragging_moves_objects)
{
MarkedObjectManyOfSame marked_object;
wxGetApp().FindMarkedObject(button_down_point, &marked_object);
if(marked_object.m_map.size()>0)
{
HeeksObj* object = marked_object.GetFirstOfTopOnly();
if (event.ShiftDown())
{
// Augment the marked_object list with objects that 'look' like
// the one selected.
CCorrelationTool correlate(wxGetApp().m_min_correlation_factor, wxGetApp().m_max_scale_threshold, wxGetApp().m_number_of_sample_points, wxGetApp().m_correlate_by_color );
std::list<HeeksObj *> similar_objects = correlate.SimilarSymbols( object );
std::list<HeeksObj *>::const_iterator l_itSymbol;
for (l_itSymbol = similar_objects.begin(); l_itSymbol != similar_objects.end(); l_itSymbol++)
{
HeeksObj *ob = *l_itSymbol;
if (! wxGetApp().m_marked_list->ObjectMarked(ob))
{
wxGetApp().m_marked_list->Add(ob, true);
}
} // End for
} // End if - then
while(object)
{
if(object->GetType() == GripperType)
{
wxGetApp().m_current_viewport->DrawFront();
wxGetApp().drag_gripper = (Gripper*)object;
wxGetApp().m_digitizing->SetOnlyCoords(wxGetApp().drag_gripper, true);
wxGetApp().m_digitizing->digitize(button_down_point);
wxGetApp().grip_from = wxGetApp().m_digitizing->digitized_point.m_point;
wxGetApp().grip_to = wxGetApp().grip_from;
double from[3];
from[0] = wxGetApp().grip_from.X();
from[1] = wxGetApp().grip_from.Y();
from[2] = wxGetApp().grip_from.Z();
wxGetApp().drag_gripper->OnGripperGrabbed(wxGetApp().m_marked_list->list(), true, from);
wxGetApp().grip_from = gp_Pnt(from[0], from[1], from[2]);
wxGetApp().m_current_viewport->EndDrawFront();
return;
}
object = marked_object.Increment();
}
}
}
}
if(event.MiddleDown())
{
button_down_point = wxPoint(event.GetX(), event.GetY());
CurrentPoint = button_down_point;
wxGetApp().m_current_viewport->StoreViewPoint();
wxGetApp().m_current_viewport->m_view_point.SetStartMousePoint(button_down_point);
}
if(event.LeftUp())
{
if(wxGetApp().drag_gripper)
{
double to[3], from[3];
wxGetApp().m_digitizing->digitize(wxPoint(event.GetX(), event.GetY()));
extract(wxGetApp().m_digitizing->digitized_point.m_point, to);
wxGetApp().grip_to = wxGetApp().m_digitizing->digitized_point.m_point;
extract(wxGetApp().grip_from, from);
wxGetApp().drag_gripper->OnGripperReleased(from, to);
wxGetApp().m_digitizing->SetOnlyCoords(wxGetApp().drag_gripper, false);
wxGetApp().drag_gripper = NULL;
}
else if(window_box_exists)
{
if(!event.ControlDown())wxGetApp().m_marked_list->Clear(true);
if(window_box.width > 0){
// only select objects which are completely within the window
MarkedObjectManyOfSame marked_object;
wxGetApp().m_marked_list->ObjectsInWindow(window_box, &marked_object, false);
std::set<HeeksObj*> obj_set;
for(HeeksObj* object = marked_object.GetFirstOfTopOnly(); object; object = marked_object.Increment())if(object->GetType() != GripperType)
obj_set.insert(object);
int bottom = window_box.y;
int top = window_box.y + window_box.height;
int height = abs(window_box.height);
if(top < bottom)
{
int temp = bottom;
bottom = top;
top = temp;
}
wxRect strip_boxes[4];
// top
strip_boxes[0] = wxRect(window_box.x - 1, top, window_box.width + 2, 1);
// bottom
strip_boxes[1] = wxRect(window_box.x - 1, bottom - 1, window_box.width + 2, 1);
// left
strip_boxes[2] = wxRect(window_box.x - 1, bottom, 1, height);
// right
strip_boxes[3] = wxRect(window_box.x + window_box.width, bottom, 1, height);
for(int i = 0; i<4; i++)
{
MarkedObjectManyOfSame marked_object2;
wxGetApp().m_marked_list->ObjectsInWindow(strip_boxes[i], &marked_object2, false);
for(HeeksObj* object = marked_object2.GetFirstOfTopOnly(); object; object = marked_object2.Increment())if(object->GetType() != GripperType)
obj_set.erase(object);
}
std::list<HeeksObj*> obj_list;
for(std::set<HeeksObj*>::iterator It = obj_set.begin(); It != obj_set.end(); It++)
{
if(!event.ControlDown() || !wxGetApp().m_marked_list->ObjectMarked(*It))obj_list.push_back(*It);
}
wxGetApp().m_marked_list->Add(obj_list, true);
}
else{
// select all the objects in the window, even if only partly in the window
MarkedObjectManyOfSame marked_object;
wxGetApp().m_marked_list->ObjectsInWindow(window_box, &marked_object, false);
std::list<HeeksObj*> obj_list;
for(HeeksObj* object = marked_object.GetFirstOfTopOnly(); object; object = marked_object.Increment())
{
if(object->GetType() != GripperType && (!event.ControlDown() || !wxGetApp().m_marked_list->ObjectMarked(object)))
obj_list.push_back(object);
}
wxGetApp().m_marked_list->Add(obj_list, true);
}
wxGetApp().m_current_viewport->DrawWindow(window_box, true); // undraw the window
window_box_exists = false;
}
else
{
// select one object
m_last_click_point = CClickPoint();
MarkedObjectOneOfEach marked_object;
wxGetApp().FindMarkedObject(wxPoint(event.GetX(), event.GetY()), &marked_object);
if(marked_object.m_map.size()>0){
HeeksObj* previously_marked = NULL;
if(wxGetApp().m_marked_list->size() == 1)
{
previously_marked = *(wxGetApp().m_marked_list->list().begin());
}
HeeksObj* o = marked_object.GetFirstOfTopOnly();
unsigned long depth = marked_object.GetDepth();
HeeksObj* object = o;
while(o)
{
if(o == previously_marked)
{
object = o;
break;
}
o = marked_object.Increment();
if(o)
{
// prefer highest order objects
if(o->GetType() < object->GetType())object = o;
}
}
if(!event.ShiftDown() && !event.ControlDown())
{
wxGetApp().m_marked_list->Clear(true);
}
if (wxGetApp().m_marked_list->ObjectMarked(object))
{
if (!event.ShiftDown())
{
wxGetApp().m_marked_list->Remove(object, true);
}
}
else
{
wxGetApp().m_marked_list->Add(object, true);
m_last_click_point = CClickPoint(wxPoint(event.GetX(), event.GetY()), depth);
gp_Lin ray = wxGetApp().m_current_viewport->m_view_point.SightLine(wxPoint(event.GetX(), event.GetY()));
double ray_start[3], ray_direction[3];
extract(ray.Location(), ray_start);
extract(ray.Direction(), ray_direction);
object->SetClickMarkPoint(&marked_object, ray_start, ray_direction);
}
}
else
{
wxGetApp().m_marked_list->Clear(true);
}
}
if(m_just_one && m_doing_a_main_loop && (wxGetApp().m_marked_list->size() > 0))
{
wxGetApp().ExitMainLoop();
}
else
{
wxGetApp().m_current_viewport->m_need_refresh = true;
}
}
else if(event.RightUp())
{
MarkedObjectOneOfEach marked_object;
wxGetApp().FindMarkedObject(wxPoint(event.GetX(), event.GetY()), &marked_object);
wxGetApp().DoDropDownMenu(wxGetApp().m_frame->m_graphics, wxPoint(event.GetX(), event.GetY()), &marked_object, false, event.ControlDown());
}
else if(event.Dragging())
{
if(event.MiddleIsDown())
{
wxPoint dm;
dm.x = event.GetX() - CurrentPoint.x;
dm.y = event.GetY() - CurrentPoint.y;
if(wxGetApp().ctrl_does_rotate == event.ControlDown())
{
if(wxGetApp().m_rotate_mode)
{
wxGetApp().m_current_viewport->m_view_point.Turn(dm);
}
else
{
wxGetApp().m_current_viewport->m_view_point.TurnVertical(dm);
}
}
else
{
wxGetApp().m_current_viewport->m_view_point.Shift(dm, wxPoint(event.GetX(), event.GetY()));
}
wxGetApp().m_current_viewport->m_need_update = true;
wxGetApp().m_current_viewport->m_need_refresh = true;
}
else if(event.LeftIsDown())
{
if(wxGetApp().drag_gripper)
{
double to[3], from[3];
wxGetApp().m_digitizing->digitize(wxPoint(event.GetX(), event.GetY()));
extract(wxGetApp().m_digitizing->digitized_point.m_point, to);
wxGetApp().grip_to = wxGetApp().m_digitizing->digitized_point.m_point;
extract(wxGetApp().grip_from, from);
wxGetApp().drag_gripper->OnGripperMoved(from, to);
wxGetApp().grip_from = gp_Pnt(from[0], from[1], from[2]);
wxGetApp().grip_from = make_point(from);
}
else if(abs(button_down_point.x - event.GetX())>2 || abs(button_down_point.y - event.GetY())>2)
{
if(wxGetApp().m_dragging_moves_objects && !window_box_exists)
{
std::list<HeeksObj*> selected_objects_dragged;
wxGetApp().m_show_grippers_on_drag = true;
if( wxGetApp().m_marked_list->list().size() > 0)
{
selected_objects_dragged = wxGetApp().m_marked_list->list();
}
else
{
MarkedObjectManyOfSame marked_object;
wxGetApp().FindMarkedObject(button_down_point, &marked_object);
if(marked_object.m_map.size()>0){
HeeksObj* object = marked_object.GetFirstOfTopOnly();
double min_depth = 0.0;
HeeksObj* closest_object = NULL;
while(object)
{
double depth = marked_object.GetDepth();
if(closest_object == NULL || depth<min_depth)
{
min_depth = depth;
closest_object = object;
}
object = marked_object.Increment();
}
if(selected_objects_dragged.size() == 0 && closest_object){
selected_objects_dragged.push_back(closest_object);
wxGetApp().m_show_grippers_on_drag = false;
}
}
}
if(selected_objects_dragged.size() > 0)
{
wxGetApp().drag_gripper = &drag_object_gripper;
wxGetApp().m_digitizing->SetOnlyCoords(wxGetApp().drag_gripper, true);
wxGetApp().m_digitizing->digitize(button_down_point);
wxGetApp().grip_from = wxGetApp().m_digitizing->digitized_point.m_point;
wxGetApp().grip_to = wxGetApp().grip_from;
double from[3];
from[0] = wxGetApp().grip_from.X();
from[1] = wxGetApp().grip_from.Y();
from[2] = wxGetApp().grip_from.Z();
wxGetApp().drag_gripper->OnGripperGrabbed(selected_objects_dragged, wxGetApp().m_show_grippers_on_drag, from);
wxGetApp().grip_from = gp_Pnt(from[0], from[1], from[2]);
double to[3];
wxGetApp().m_digitizing->digitize(wxPoint(event.GetX(), event.GetY()));
extract(wxGetApp().m_digitizing->digitized_point.m_point, to);
wxGetApp().grip_to = wxGetApp().m_digitizing->digitized_point.m_point;
extract(wxGetApp().grip_from, from);
wxGetApp().drag_gripper->OnGripperMoved(from, to);
wxGetApp().grip_from = gp_Pnt(from[0], from[1], from[2]);
return;
}
}
// do window selection
if(!m_just_one)
{
//wxGetApp().m_frame->m_graphics->SetCurrent();
wxGetApp().m_current_viewport->SetXOR();
if(window_box_exists)wxGetApp().m_current_viewport->DrawWindow(window_box, true); // undraw the window
window_box.x = button_down_point.x;
window_box.width = event.GetX() - button_down_point.x;
window_box.y = wxGetApp().m_current_viewport->GetViewportSize().GetHeight() - button_down_point.y;
window_box.height = button_down_point.y - event.GetY();
wxGetApp().m_current_viewport->DrawWindow(window_box, true);// draw the window
wxGetApp().m_current_viewport->EndXOR();
window_box_exists = true;
}
}
}
CurrentPoint = wxPoint(event.GetX(), event.GetY());
}
else if(event.Moving())
{
CurrentPoint = wxPoint(event.GetX(), event.GetY());
}
if(event.GetWheelRotation() != 0)
{
double wheel_value = (double)(event.GetWheelRotation());
double multiplier = wheel_value /1000.0, multiplier2;
if(wxGetApp().mouse_wheel_forward_away)multiplier = -multiplier;
// make sure these are actually inverses, so if you
// zoom in and out the same number of steps, you'll be
// at the same zoom level again
if(multiplier > 0) {
multiplier2 = 1 + multiplier;
} else {
multiplier2 = 1/(1 - multiplier);
}
wxSize client_size = wxGetApp().m_current_viewport->GetViewportSize();
double pixelscale_before = wxGetApp().GetPixelScale();
wxGetApp().m_current_viewport->m_view_point.Scale(multiplier2);
double pixelscale_after = wxGetApp().GetPixelScale();
double event_x = event.GetX();
double event_y = event.GetY();
double center_x = client_size.GetWidth() / 2.;
double center_y = client_size.GetHeight() / 2.;
// how many pixels are we from the center (the center
// is the point that doesn't move when you zoom)?
double px = event_x - center_x;
double py = event_y - center_y;
// that number of pixels represented how many mm
// before and after the zoom ...
double xbefore = px / pixelscale_before;
double ybefore = py / pixelscale_before;
double xafter = px / pixelscale_after;
double yafter = py / pixelscale_after;
// which caused a change in how many mm at that point
// on the screen?
double xchange = xafter - xbefore;
double ychange = yafter - ybefore;
// and how many pixels worth of motion is that?
double x_moved_by = xchange * pixelscale_after;
double y_moved_by = ychange * pixelscale_after;
// so move that many pixels to keep the coordinate
// under the cursor approximately the same
wxGetApp().m_current_viewport->m_view_point.Shift(wxPoint((int)x_moved_by, (int)y_moved_by), wxPoint(0, 0));
wxGetApp().m_current_viewport->m_need_refresh = true;
}
}
void StandardEstimator::correlate_rr(ChainingMesh& mesh1, ChainingMesh& mesh2, double vmax) {
correlate(mesh1, mesh2, rr, vmax);
}