bool EyeTrackerCalibration::calibrate(const CalibrationData & calibrationData) { if(calibrationData.size() == 0) return false; size_t totalCalibrationPointsNum = 0; for(const auto & p: calibrationData) totalCalibrationPointsNum += p.eyePositions.size(); if(totalCalibrationPointsNum < 10) return false; std::vector<cv::Point2d> eyePositions; std::vector<cv::Point2d> screenPositions; if(m_dataPreprocessingType == NoPreprocessing) { eyePositions.resize(totalCalibrationPointsNum); screenPositions.resize(totalCalibrationPointsNum); size_t i = 0; for(const auto & p: calibrationData) { const auto screenPos = p.screenPoint; for(const auto & eyePos: p.eyePositions) { eyePositions[i] = eyePos; screenPositions[i] = screenPos; ++i; } } } else if(m_dataPreprocessingType == MeanPoint) { eyePositions.resize(calibrationData.size()); screenPositions.resize(calibrationData.size()); size_t i = 0; for(const auto & p: calibrationData) { double mean_x = 0.0; double mean_y = 0.0; for(const auto & eyePos: p.eyePositions) { mean_x += eyePos.x; mean_y += eyePos.y; } mean_x /= p.eyePositions.size(); mean_y /= p.eyePositions.size(); eyePositions[i] = cv::Point2d(mean_x, mean_y); screenPositions[i] = p.screenPoint; i++; } } return estimateParameters(eyePositions, screenPositions); }
bool processFrame(NUI_Controller* mNui) { optimizeDepthMap(); getSphereSizes(mNui); measureBody(mNui); estimateParameters(); return true; }
void SQFitting::estimateInitialParameters(pcl::PointCloud<pcl::PointXYZ>& cloud, SQParameters& sqParams) { double minError = 1000000; int maxEigenVector = 0; int retVal; for (int i = 0; i < 3; i++) { retVal = estimateParameters(cloud, sqParams, i); if (retVal == -1) { printf("Error: At least 13 data points must support the fit!\n"); } double error = 0; error = minProcess.errorFunc(cloud, sqParams); if (error < minError) { minError = error; maxEigenVector = i; } } retVal = estimateParameters(cloud, sqParams, maxEigenVector); }
void SQFitting::performShapeFitting(const pcl::PointCloud<pcl::PointXYZ>& cloud, SQParameters& initParams, SQParameters& bestParams) { pcl::PointCloud<pcl::PointXYZ> tempSQCloud; SQParameters unrefinedParams, refinedParams; double error, minError = 1.0e13;; int ret = -1, eigenVect; int iterations; // Get best eigenvector iterations = initParams.min.iterations; initParams.min.iterations = 20; for (int i = 0; i < 3; i++) { ret = estimateParameters(cloud, initParams, i); if (ret == -1) { printf("Error: Cannot perform superquadric fitting!\n"); } error = recoverParameters(cloud, initParams); if (error < minError) { minError = error; eigenVect = i; } } initParams.min.iterations = iterations; initParams.principalAxis = eigenVect; ret = estimateParameters(cloud, initParams, eigenVect); if (ret == -1) { printf("Error: Cannot perform superquadric fitting!\n"); error = 100000; } else { error = recoverParameters(cloud, initParams); } initParams.copyTo(bestParams); }
bool processFrame(xn::DepthGenerator* dpg,xn::UserGenerator* ug,XnUserID userID) { if (!dImage) { dImage=cvCreateImage(dSize,IPL_DEPTH_16U,1); uImage=cvCreateImage(dSize,IPL_DEPTH_8U,1); showImage=cvCreateImage(dSize,IPL_DEPTH_8U,1); } cvSetZero(dImage); cvSetZero(uImage); if (!convertMetaDataToIpl( dpg,ug,userID)) //Convert Xn Matrices to OpenCV Matrices for easier calculation. return false; optimizeDepthMap(); getSphereSizes(dpg,ug,userID); measureBody(dpg,ug,userID); estimateParameters(); return true; }
// estimate the HMM parameters void MAPEstimator::estimateParameters(MAccumulatorPhysical &mAccumulator, float fPriorKnowledgeWeight) { for(int i=0 ; i < m_iHMMStates ; ++i) { bool bData = false; int iComponents = m_hmmStates[i]->getMixture().getNumberComponents(); Accumulator **accumulators = new Accumulator*[iComponents]; for(int g=0 ; g < iComponents ; ++g) { MAccumulatorPhysical::iterator it = mAccumulator.find(Accumulator::getPhysicalAccumulatorKey(i,g)); if (it != mAccumulator.end()) { accumulators[g] = it->second; bData = true; } else { accumulators[g] = NULL; } } // is there data to update the HMM-state if (bData) { estimateParameters(m_hmmStates[i],accumulators,fPriorKnowledgeWeight); } delete [] accumulators; } }
int CExpNet::estimateNodeProb(int nnode, int *parnodes, int numpars, double *psamples, int nsamples) { int res, i, j, ic, iC; int *pcats, numcats; double fdiff, fdiffsum, fsum, *paux; double *pnodeprob; PROB_LIST<double>* pProbList; if(nnode < 0 || nnode >= m_numNodes || !m_loglambdas || !psamples || nsamples < 1) return ERR_CATNET_PARAM; pProbList = (PROB_LIST<double>*)getNodeProb(nnode); pnodeprob = pProbList->pProbs; numcats = m_numCategories[nnode]; if(numpars <= 0 || !parnodes) { fsum = 0; for(ic = 0; ic < numcats; ic++) { fdiffsum = 0; for(j = 0; j < nsamples; j++) { fdiff = (psamples[j * m_numNodes + nnode] * m_loglambdas[nnode][numcats+ic] + m_loglambdas[nnode][ic]); // P(Y_i|X_i=c) fdiffsum += exp(-fdiff); } pnodeprob[ic] = fdiffsum; fsum += fdiffsum; } if(fsum > 0) fsum = 1/fsum; for(ic = 0; ic < numcats; ic++) pnodeprob[ic] *= fsum; return ERR_CATNET_OK; } // at this point m_nBlockSizes == numnodes and m_pBlockSizes are filled m_parCatSetSize = 1; for(i = 0; i < numpars; i++) { m_parCatSetSize *= m_numCategories[parnodes[i]]; } if(m_pcC) CATNET_FREE(m_pcC); m_pcC = 0; if(m_qcC) CATNET_FREE(m_qcC); m_qcC = 0; // m_pcC returns marginal P(X_{Pa_i}) m_pcC = (double*)CATNET_MALLOC(m_parCatSetSize*sizeof(double)); pcats = (int*)CATNET_MALLOC(m_parCatSetSize*numpars*sizeof(int)); if(!m_pcC || !pcats) return ERR_CATNET_MEM; memset(m_pcC, 0, m_parCatSetSize*sizeof(double)); if(!m_pCatnetSamples || m_nCatnetSamples < 1) { // find the exact joint probability for small sets res = marginalProb(parnodes, numpars); if(res != ERR_CATNET_OK) { CATNET_FREE(pcats); return res; } if(m_margProbSize != m_parCatSetSize) { CATNET_FREE(pcats); return ERR_CATNET_PROC; } for(ic = 0; ic < m_parCatSetSize; ic++) { // determine the category indices fsum = ic; for(i = 0; i < numpars; i++) { pcats[ic * numpars + i] = (int)(fsum/m_pBlockSizes[i]); fsum -= pcats[ic*numpars + i] * m_pBlockSizes[i]; } m_pcC[ic] = getCatProb(pcats + ic * numpars, numpars); } } else { if(!m_pBlockSizes || m_nBlockSizes < numpars) { if(m_pBlockSizes) CATNET_FREE(m_pBlockSizes); m_nBlockSizes = numpars; m_pBlockSizes = (int*) CATNET_MALLOC(m_nBlockSizes * sizeof(int)); } m_pBlockSizes[numpars - 1] = 1; for (i = numpars - 2; i >= 0; i--) { m_pBlockSizes[i] = m_pBlockSizes[i + 1] * m_numCategories[parnodes[i + 1]]; } for(j = 0; j < m_nCatnetSamples; j++) { ic = 0; for (i = 0; i < numpars; i++) ic += (m_pBlockSizes[i] * m_pCatnetSamples[j * m_numNodes + parnodes[i]]); m_pcC[ic] += 1; } fsum = 1/(double)m_nCatnetSamples; for(ic = 0; ic < m_parCatSetSize; ic++) m_pcC[ic] *= fsum; for(ic = 0; ic < m_parCatSetSize; ic++) { // determine the category indices fsum = ic; for(i = 0; i < numpars; i++) { pcats[ic * numpars + i] = (int)(fsum/m_pBlockSizes[i]); fsum -= pcats[ic*numpars + i] * m_pBlockSizes[i]; } } } paux = (double*)CATNET_MALLOC(m_parCatSetSize*sizeof(double)); m_qcC = (double*)CATNET_MALLOC(m_parCatSetSize*nsamples*sizeof(double)); for (j = 0; j < nsamples; j++) { fsum = 0; for(ic = 0; ic < m_parCatSetSize; ic++) { fdiffsum = 0; for(i = 0; i < numpars; i++) { fdiffsum += (psamples[j * m_numNodes + parnodes[i]] * m_loglambdas[parnodes[i]][m_numCategories[parnodes[i]]+pcats[ic*numpars + i]] + m_loglambdas[parnodes[i]][pcats[ic*numpars + i]]); } paux[ic] = m_pcC[ic]*exp(-fdiffsum); fsum += paux[ic]; } if(fsum > 0) fsum = 1/fsum; else fsum = FLT_MAX; // set P(X_{Pa_i}|Y_{Pa_i}) in m_qcC for(ic = 0; ic < m_parCatSetSize; ic++) m_qcC[j*m_parCatSetSize + ic] = fsum*paux[ic]; } for(iC = 0; iC < m_parCatSetSize; iC++) { fsum = 0; for(ic = 0; ic < numcats; ic++) { fdiffsum = 0; for(j = 0; j < nsamples; j++) { fdiff = (psamples[j * m_numNodes + nnode] * m_loglambdas[nnode][numcats+ic] + m_loglambdas[nnode][ic]); // P(Y_i|X_i=c)P(Pa_i=C|Y_{Pa_i}) fdiffsum += exp(-fdiff)*m_qcC[j*m_parCatSetSize + iC]; } pnodeprob[iC*numcats + ic] = fdiffsum; fsum += fdiffsum; } if(fsum > 0) fsum = 1/fsum; for(ic = 0; ic < numcats; ic++) pnodeprob[iC*numcats + ic] *= fsum; } CATNET_FREE(pcats); CATNET_FREE(paux); // we don't need them any more if(m_pcC) CATNET_FREE(m_pcC); m_pcC = 0; if(m_qcC) CATNET_FREE(m_qcC); m_qcC = 0; // update the betas and sigma res = findNodeMarginalProb(nnode, psamples, nsamples); if(res != ERR_CATNET_OK) return res; estimateParameters(nnode, psamples, nsamples, m_lambdas[nnode], m_loglambdas[nnode]); if(m_pc) CATNET_FREE(m_pc); m_pc = 0; if(m_qc) CATNET_FREE(m_qc); m_qc = 0; return ERR_CATNET_OK; }
int main(int argc, char* argv[]) { // parameters that you can set. string infF = ""; string infR = ""; string infPath = ""; string outI = "oIter.txt"; string outP = "oPar.txt"; string outCorr = "oCorr.txt"; string outCnt = "oCnt.txt"; string outPath = ""; string outStub = ""; //bool useemp = false; double truncLimit = 0.99; int verbose = 3; int lambdaD = 147; int minD = 130; int maxD = 180; int sampleSize = -1; int burnins = 1000; int iterations = 10000; int seed = -1; // Unknowns double lambdaF[2]; double lambdaR[2]; double pF[2]; double pR[2]; /* * Read and check input parameters */ string errorLine = "usage " + string(argv[0]) + " [parameters] \n" + "\t-iF <forward reads binary file> \n" + "\t-iR <reverse reads binary file> \n" + "\t-iPath <path to forward and reverse reads binary files \n" + " (overrides iR and iF if set)> \n" + "\t-oPath <outdirectory, where all output files are put. \n" + "\t NOT created - needs to exists. Defaults to where \n"+ "\t the program is executed from.>\n" + "\t-oStub <outfile names stub, \n" + "\t defaults -oIter to <-oStub_>oIter.txt, \n" + "\t defaults -oPar to <-oStub_>oPar.txt> \n" + "\t defaults -oCorr to <-oStub_>oCorr.txt> \n" + "\t defaults -oCnt to <-oStub_>oCnt.txt> \n" + "\t-oIter <outfile, where to write MCMC parameter\n" + "\t estimates for each iteration,default generated from -oStub>\n" + "\t-oPar <parameter-file, where to write MCMC\n" + "\t final parameter estimates, default generated from -oStub> \n" + /* "\t-oCorr <out-file, lists correlation coefficients between forward\n" + "\t and reverse reads in [mind,maxd] \n" + "\t default generated from -oStub> \n" + */ "\t-oCnt <out-file, lists forward and reverse read count frequencies\n" + "\t default generated from -oStub> \n" + "\t-trunc <truncation limit in (0,1], default 0.99.> \n" + "\t-size <sample size, default all observations.> \n" + "\t-burnin <number of MCMC burnin iterations,\n" + "\t default 1000.> \n" + "\t-iter <number of MCMC iterations\n" + "\t (non-burnin iterations), default 10000.> \n" + "\t-seed <set seed to random number generator.> \n" + "\t-ld <distance-lambda, default 147 bp.> \n" + "\t-mind <min distance, default 130 bp.> \n" + "\t-maxd <max distance, default 180 bp.> \n" + /* "\t-useemp <toggle use of data-driven distance distribution, or poisson\n" + "\t around distance-lambda. default off.>\n" + */ "\t-v <verbose level 0-3. default 2>"; bool fail = false; string failmessage = ""; for (int i = 1; i < argc; i++) { if(strcmp(argv[i],"-iF") == 0) infF.assign(argv[++i]); else if(strcmp(argv[i],"-iR") == 0) infR.assign(argv[++i]); else if(strcmp(argv[i],"-iPath") == 0) infPath.assign(argv[++i]); else if(strcmp(argv[i],"-oPath") == 0) outPath.assign(argv[++i]); else if(strcmp(argv[i],"-oStub") == 0) outStub.assign(argv[++i]); else if(strcmp(argv[i],"-oIter") == 0) outI.assign(argv[++i]); else if(strcmp(argv[i],"-oPar") == 0) outP.assign(argv[++i]); /* else if(strcmp(argv[i],"-oCorr") == 0) outCorr.assign(argv[++i]); */ else if(strcmp(argv[i],"-oCnt") == 0) outCnt.assign(argv[++i]); /* else if (strcmp(argv[i],"-useemp") == 0) useemp = true; */ else if (strcmp(argv[i],"-v") == 0) verbose = atoi(argv[++i]); else if (strcmp(argv[i],"-seed") == 0) seed = atoi(argv[++i]); else if (strcmp(argv[i],"-trunc") == 0) truncLimit = atof(argv[++i]); else if (strcmp(argv[i],"-size") == 0) sampleSize = atoi(argv[++i]); else if (strcmp(argv[i],"-burnin") == 0) burnins = atoi(argv[++i]); else if (strcmp(argv[i],"-iter") == 0) iterations = atoi(argv[++i]); else if (strcmp(argv[i],"-ld") == 0) lambdaD = atoi(argv[++i]); else if (strcmp(argv[i],"-mind") == 0) minD = atoi(argv[++i]); else if (strcmp(argv[i],"-maxd") == 0) maxD = atoi(argv[++i]); else { failmessage.assign("Unknown argument: "); failmessage.append(argv[i]); failmessage.append("\n"); fail = true; } } if (truncLimit <= 0 || truncLimit > 1) { failmessage.append("-trunc value does not make sense.\n"); fail = true; } bool infPathSpec = false; if (strcmp(infPath.c_str(), "") != 0) { infPathSpec = true; DIR *d = opendir(infPath.c_str()); if(d) { closedir(d); } else { failmessage.append("-iPath does not exist.\n"); fail = true; } } if (strcmp(infF.c_str(), "") == 0) { if (!infPathSpec) { failmessage.append("-iF or -iPath must be specified.\n"); fail = true; } } if (strcmp(infR.c_str(), "") == 0) { if (!infPathSpec) { failmessage.append("-iR or -iPath must be specified.\n"); fail = true; } } if (strcmp(outI.c_str(), "") == 0) { failmessage.append("invalid -oIter.\n"); fail = true; } if (strcmp(outP.c_str(), "") == 0) { failmessage.append("invalid -oPar.\n"); fail = true; } if (strcmp(outCorr.c_str(), "") == 0) { failmessage.append("invalid -oCorr.\n"); fail = true; } if (strcmp(outCnt.c_str(), "") == 0) { failmessage.append("invalid -oCnt.\n"); fail = true; } if (strcmp(outPath.c_str(), "") != 0) { DIR* d = opendir(outPath.c_str()); if(d) { closedir(d); } else { failmessage.append("-oPath does not exist.\n"); fail = true; } } int infFCnt = 1; if (infPathSpec) { infFCnt = countFiles(infPath); if (infFCnt < 1) { failmessage.append("ERROR: infile path \""); failmessage.append(infPath.c_str()); failmessage.append("\" does not contain a positive number of F and R binary files, aborting.\n"); fail = true; } } if (fail) { cerr << endl << failmessage.c_str() << endl << errorLine << endl; return(-1); } if(strcmp(outStub.c_str(),"") != 0) { outI = outStub + "_" + outI; outP = outStub + "_" + outP; outCorr = outStub + "_" + outCorr; outCnt = outStub + "_" + outCnt; } if(strcmp(outPath.c_str(),"") != 0) { outI = outPath + outI; outP = outPath + outP; outCorr = outPath + outCorr; outCnt = outPath + outCnt; } if (seed < -1) seed = -1; ifstream iff[infFCnt]; ifstream ifr[infFCnt]; string fileNames[infFCnt]; if (infPathSpec) { if (openFiles(infPath, iff, ifr, fileNames) != infFCnt) { failmessage.append("ERROR: all files in \""); failmessage.append(infPath.c_str()); failmessage.append("\" could not be opened, aborting.\n"); fail = true; } } else { iff[0].open(infF.c_str(),ios::binary); ifr[0].open(infR.c_str(),ios::binary); if (iff[0].fail()) { failmessage.append("ERROR: Forward reads binary file \""); failmessage.append(infF.c_str()); failmessage.append("\" could not be opened, aborting.\n"); fail = true; } if (ifr[0].fail()) { failmessage.append("ERROR: Reverse reads binary file \""); failmessage.append(infR.c_str()); failmessage.append("\" could not be opened, aborting.\n"); fail = true; } } /* iff.open(infF.c_str(),ios::binary); if (iff.fail()) { failmessage.append("ERROR: Forward reads binary file \""); failmessage.append(infF.c_str()); failmessage.append("\" could not be opened, aborting.\n"); fail = true; } ifstream ifr; ifr.open(infR.c_str(),ios::binary); if (ifr.fail()) { failmessage.append("ERROR: Reverse reads binary file \""); failmessage.append(infR.c_str()); failmessage.append("\" could not be opened, aborting.\n"); fail = true; } */ ofstream ofi; ofi.open(outI.c_str(),ios::trunc); if (ofi.fail()) { failmessage.append("ERROR: Output file \""); failmessage.append(outI.c_str()); failmessage.append("\" could not be created, aborting.\n"); fail = true; } ofstream ofc; ofc.open(outCorr.c_str(),ios::trunc); if (ofc.fail()) { failmessage.append("ERROR: Output file \""); failmessage.append(outCorr.c_str()); failmessage.append("\" could not be created, aborting.\n"); fail = true; } ofstream ofcnt; ofcnt.open(outCnt.c_str(),ios::trunc); if (ofcnt.fail()) { failmessage.append("ERROR: Output file \""); failmessage.append(outCnt.c_str()); failmessage.append("\" could not be created, aborting.\n"); fail = true; } ofstream ofp; int truncValF,truncValR; int estMinD = minD, estMaxD = maxD, estLambdaD = lambdaD; double* distDens; vector<string> distDensV; ofp.open(outP.c_str(),ios::trunc); if (ofp.fail()) { failmessage.append("ERROR: Paramater file \""); failmessage.append(outP.c_str()); failmessage.append("\" could not be created, aborting.\n"); fail = true; } if (fail) { cerr << endl << failmessage.c_str() << endl << errorLine << endl; return(-1); } ofi << "! Command:"; for (int i = 0; i < argc; i++) ofi << " " << argv[i]; ofi << endl; vlevel = verbose; /* * Check file lengths */ int minPos[infFCnt], maxPos[infFCnt], minF[infFCnt], maxF[infFCnt], minR[infFCnt], maxR[infFCnt]; checkFileLengths(iff, ifr, minPos, maxPos, minR, minF, maxF, maxR, infFCnt); /* * Estimate parameters */ ofp << "! Command:"; for (int i = 0; i < argc; i++) ofp << " " << argv[i]; ofp << endl; vcerr(1) << "*** Identifying truncation limits and data distributions ***" << endl; calculateTruncVal(iff,ifr,&ofcnt, &truncValF, &truncValR, truncLimit, minPos, maxPos, minR, minF, maxF, maxR, infFCnt); ofcnt.close(); distDens = new double[maxD-minD+1]; if (infFCnt == 1) { estimateDistance(&iff[0],&ifr[0],&ofc, &estMinD, &estMaxD, &estLambdaD, minD, maxD, minPos[0], maxPos[0], minR[0], minF[0], maxF[0], maxR[0], truncValF, truncValR,distDens,0.3); ofc.close(); } /* if (useemp) { vcerr(2) << "\t* Estimating forward-reverse distance" << endl; estimateDistance(&iff,&ifr,&ofc, &estMinD, &estMaxD, &estLambdaD, minD, maxD, minPos, maxPos, minR, minF, maxF, maxR, truncValF, truncValR,distDens,0.3); ofc.close(); } else { */ for (int dist = minD; dist <= maxD; dist++) distDens[dist-minD] = gsl_ran_poisson_pdf(dist, lambdaD); /* } */ vcerr(1) << "*** Parameter estimation ***" << endl; if (estimateParameters(iff,ifr,&ofi, estMinD,estMaxD,estLambdaD, pF, pR, lambdaF, lambdaR, sampleSize,burnins,iterations, seed, minPos, maxPos, minR, minF, maxF, maxR, truncValF, truncValR, infFCnt) < 0) { cerr << "ERROR: estimateParameters failed, aborting." << endl; for (int i=0; i<infFCnt; i++) { iff[i].close(); ifr[i].close(); } ofi.close(); ofp.close(); delete[] distDens; return(-1); } else { if (writeParameters(&ofp, pF, pR, lambdaF, lambdaR, lambdaD, truncValF, truncValR, minD, maxD, estLambdaD, estMinD, estMaxD, distDens) < 0) { cerr << "WARNING: could not write parameters to file, skipping." << endl; } } ofi.close(); ofp.close(); for (int i=0; i<infFCnt; i++) { iff[i].close(); ifr[i].close(); } delete[] distDens; vcerr(3) << setprecision(3); vcerr(3) << endl << "\t\tParameter estimates:" << endl; vcerr(3) << "\t\tpF: S = " << pF[0] << " NotS = " << pF[1] << endl; vcerr(3) << "\t\tpR: E = " << pR[0] << " NotE = " << pR[1] << endl; vcerr(3) << "\t\tlambdaF: S = " << lambdaF[0] << " NotS = " << lambdaF[1] << endl; vcerr(3) << "\t\tlambdaR: E = " << lambdaR[0] << " NotE = " << lambdaR[1] << endl; vcerr(3) << "\t\testMinD: = " << estMinD << " estMaxd = " << estMaxD << endl; }