pcl::CorrespondencesPtr matchSIFT(const cv::Mat &descr1, const cv::Mat &descr2) { pcl::CorrespondencesPtr corrs (new pcl::Correspondences()); cv::flann::Index sift_tree; cv::flann::LinearIndexParams params; #ifdef opencv_miniflann_build_h extFlannIndexBuild(sift_tree,descr1, params); #else sift_tree.build(descr1, params); #endif for( int i = 0 ; i < descr2.rows ; i++ ) { std::vector<int> idxs; std::vector<float> dists; sift_tree.knnSearch(descr2.row(i), idxs, dists, 2, cv::flann::SearchParams()); if( dists[0] / dists[1] < SIFT_RATIO ) { pcl::Correspondence cur_corr (idxs[0], i, 0); //std::cerr << idxs[0] << " " << i << std::endl; corrs->push_back (cur_corr); } } return corrs; }
void Writehisto(int i, int start, int end){ string trig="dAu200all"; TString trigtype(trig); TString name; // name = "treepass.lst"; // name = "proSTEGvnmult300.lst"; name = "tree.lst"; // std::vector<TString> s; // s.push_back("vndata_50k_100.root"); // for(int i=start;i<end;i++){ // cout << i << endl; // Perform *pl = new Perform(readline(Form("%s",name.Data()),start,end),Form("treeout/Ana%s_%d.root",trigtype.Data(),i)); // QCumulant *pl = new QCumulant(readline(Form("%s",name.Data()),start,end),Form("treeout/MCAna_mult300_%d.root",i)); std::ifstream corrs("indexed_runlist.dat"); int index=0; int run=0; for(int irun=0;irun<i+1;irun++){ corrs>>index>>run; } QCumulantOnline *pl = new QCumulantOnline(readline(Form("%s",name.Data()),i,start,end),Form("testAnaFull%s_%d.root",trigtype.Data(),i)); pl->Init(); pl->SetRun(run); pl->Setweight(0); // pl->SetcalFlag(0); // pl->Inittree(); // pl->process_event(); pl->SetcalFlag(1); pl->Inittree(); pl->process_event(); pl->End(); delete pl; // } }
void Writehisto(){ int i=atoi(getenv("I")); string trig="dAu39all"; TString trigtype(trig); TString name, name1; name = "tree.lst"; name1 = "tree1.lst"; std::ifstream corrs("Run16dAu39GeV.lst"); int index=0; int run=0; for(int irun=0;irun<i+1;irun++){ corrs>>index>>run; } //EPAnaRun16alltree *pl = new EPAnaRun16alltree(readline(Form("%s",name.Data()),i), readline(Form("%s",name1.Data()),i), Form("testEPAnaFull.root")); EPAnaRun16alltree *pl = new EPAnaRun16alltree(readline(Form("%s",name.Data()),i), readline(Form("%s",name1.Data()),i), Form("/store/user/qixu/flow/Run16dAu/39GeV/EPAnaFull%s_20percutrbin_%d.root",trigtype.Data(),i)); pl->Init(); pl->Inittree(); pl->SetcalFlag(0); pl->process_event(); pl->SetcalFlag(1); pl->process_event(); pl->Getrec(); pl->SetcalFlag(2); pl->process_event(); pl->Getflt(); pl->SetcalFlag(3); pl->process_event(); pl->End(); delete pl; }
std::vector<TString> readline(char* name, int i, int ilinestart, int ilineend){ std::ifstream backstory(name); std::vector<TString> vecline; std::string line; std::ifstream corrs("indexed_runlist.dat"); int index=0; int run=0; for(int irun=0;irun<i+1;irun++){ corrs>>index>>run; } if (backstory.is_open()) if(backstory.good()){ for(int i = 0; i < ilineend; ++i){ getline(backstory, line); if(i>=ilinestart){ TString tline(line); if(!tline.Contains(Form("%d",run)))continue; vecline.push_back(tline); } } } return vecline; }
std::vector<TString> readline(char* name, int i){ std::ifstream backstory(name); std::vector<TString> vecline; std::string line; std::ifstream corrs("Run16dAu39GeV.lst"); int index=0; int run=0; for(int irun=0;irun<i+1;irun++){ corrs>>index>>run; } if (backstory.is_open()) if(backstory.good()){ while(!backstory.eof()){ getline(backstory, line); if(i>=0){ TString tline(line); if(!tline.Contains(Form("%d",run)))continue; vecline.push_back(tline); } } } return vecline; }
void CUDASolverBundling::computeMaxResidual(SolverInput& solverInput, SolverParameters& parameters, unsigned int revalidateIdx) { if (m_timer) m_timer->startEvent(__FUNCTION__); if (parameters.weightSparse > 0.0f) { evalMaxResidual(solverInput, m_solverState, m_solverExtra, parameters, NULL);//m_timer); // copy to cpu unsigned int n = (solverInput.numberOfCorrespondences + THREADS_PER_BLOCK - 1) / THREADS_PER_BLOCK; cutilSafeCall(cudaMemcpy(m_solverExtra.h_maxResidual, m_solverExtra.d_maxResidual, sizeof(float) * n, cudaMemcpyDeviceToHost)); cutilSafeCall(cudaMemcpy(m_solverExtra.h_maxResidualIndex, m_solverExtra.d_maxResidualIndex, sizeof(int) * n, cudaMemcpyDeviceToHost)); // compute max float maxResidual = 0.0f; int maxResidualIndex = 0; for (unsigned int i = 0; i < n; i++) { if (maxResidual < m_solverExtra.h_maxResidual[i]) { maxResidual = m_solverExtra.h_maxResidual[i]; maxResidualIndex = m_solverExtra.h_maxResidualIndex[i]; } } #ifdef NEW_GUIDED_REMOVE //if (solverInput.numberOfImages == 51) { // SensorData sd; sd.loadFromFile("../data/iclnuim/aliv2.sens"); // std::vector<mat4f> trajectory(solverInput.numberOfImages); // MLIB_CUDA_SAFE_CALL(cudaMemcpy(trajectory.data(), d_transforms, sizeof(mat4f)*trajectory.size(), cudaMemcpyDeviceToHost)); // sd.saveToPointCloud("debug/tmp.ply", trajectory, 0, solverInput.numberOfImages*10, 10, true); // int a = 5; //} m_maxResImPairs.clear(); if (maxResidual > GUIDED_SEARCH_MAX_RES_THRESH) { parameters.highResidualThresh = std::min(std::max(0.2f * maxResidual, 0.1f), 0.4f); collectHighResiduals(solverInput, m_solverState, m_solverExtra, parameters, m_timer); unsigned int highResCount; cutilSafeCall(cudaMemcpy(&highResCount, m_solverState.d_countHighResidual, sizeof(unsigned int), cudaMemcpyDeviceToHost)); n = std::min(highResCount, (m_maxCorrPerImage*m_maxNumberOfImages + THREADS_PER_BLOCK - 1) / THREADS_PER_BLOCK); cutilSafeCall(cudaMemcpy(m_solverExtra.h_maxResidual, m_solverExtra.d_maxResidual, sizeof(float) * n, cudaMemcpyDeviceToHost)); cutilSafeCall(cudaMemcpy(m_solverExtra.h_maxResidualIndex, m_solverExtra.d_maxResidualIndex, sizeof(int) * n, cudaMemcpyDeviceToHost)); if (n > 1) { // check high residuals with previous trajectory as reference //TODO MAKE EFFICIENT std::vector<float4x4> transforms(solverInput.numberOfImages); MLIB_CUDA_SAFE_CALL(cudaMemcpy(transforms.data(), d_transforms, sizeof(float4x4)*solverInput.numberOfImages, cudaMemcpyDeviceToHost)); std::unordered_map<vec2ui, float> residualMap; //TODO should be something better than this... std::unordered_map<vec2ui, float> allCollectedResidualMap; //debugging std::vector<EntryJ> corrs(n); for (unsigned int i = 0; i < n; i++) { MLIB_CUDA_SAFE_CALL(cudaMemcpy(corrs.data() + i, solverInput.d_correspondences + m_solverExtra.h_maxResidualIndex[i], sizeof(EntryJ), cudaMemcpyDeviceToHost)); const EntryJ& h_corr = corrs[i]; vec2ui imageIndices(h_corr.imgIdx_i, h_corr.imgIdx_j); //compute res at previous if (h_corr.imgIdx_j == solverInput.numberOfImages - 1 && std::abs((int)h_corr.imgIdx_i - (int)h_corr.imgIdx_j) > 10) { //introduced by latest image float3 prevRes = fabs(transforms[h_corr.imgIdx_i] * h_corr.pos_i - transforms[h_corr.imgIdx_j] * h_corr.pos_j); //eval new corrs with previous trajectory float prevMaxRes = fmaxf(prevRes.z, fmaxf(prevRes.x, prevRes.y)); if (prevMaxRes > 1.5f*m_solverExtra.h_maxResidual[i]) { auto it = residualMap.find(imageIndices); if (it == residualMap.end()) residualMap[imageIndices] = m_solverExtra.h_maxResidual[i]; else it->second = std::max(m_solverExtra.h_maxResidual[i], it->second); } } else if (h_corr.imgIdx_j == revalidateIdx && std::abs((int)h_corr.imgIdx_i - (int)h_corr.imgIdx_j) > 10) { //introduced by latest revalidate auto it = residualMap.find(imageIndices); if (it == residualMap.end()) residualMap[imageIndices] = m_solverExtra.h_maxResidual[i]; else it->second = std::max(m_solverExtra.h_maxResidual[i], it->second); } auto it = allCollectedResidualMap.find(imageIndices); if (it == allCollectedResidualMap.end()) allCollectedResidualMap[imageIndices] = m_solverExtra.h_maxResidual[i]; else it->second = std::max(m_solverExtra.h_maxResidual[i], it->second); } if (!residualMap.empty()) { //debug print unsigned int rep = residualMap.begin()->first.x; std::cout << "rep: (" << rep << ", " << solverInput.numberOfImages - 1 << ")" << std::endl; for (const auto& r : residualMap) m_maxResImPairs.push_back(r.first); ////one extra solve //parameters.nNonLinearIterations = 1; //solveBundlingStub(solverInput, m_solverState, parameters, m_solverExtra, NULL, m_timer); ////!!!debugging //{ // static SensorData sd; // if (sd.m_frames.empty()) sd.loadFromFile("../data/iclnuim/aliv2.sens"); // std::vector<mat4f> trajectory(solverInput.numberOfImages); // MLIB_CUDA_SAFE_CALL(cudaMemcpy(trajectory.data(), d_transforms, sizeof(mat4f)*trajectory.size(), cudaMemcpyDeviceToHost)); // sd.saveToPointCloud("debug/tmp/" + std::to_string(solverInput.numberOfImages) + "-init.ply", trajectory, 0, solverInput.numberOfImages*10, 10, true); // convertLiePosesToMatricesCU(m_solverState.d_xRot, m_solverState.d_xTrans, solverInput.numberOfImages, d_transforms, m_solverState.d_xTransformInverses); // MLIB_CUDA_SAFE_CALL(cudaMemcpy(trajectory.data(), d_transforms, sizeof(mat4f)*trajectory.size(), cudaMemcpyDeviceToHost)); // sd.saveToPointCloud("debug/tmp/" + std::to_string(solverInput.numberOfImages) + "-opt.ply", trajectory, 0, solverInput.numberOfImages*10, 10, true); // int a = 5; //} ////!!!debugging } //!!!debugging //std::vector<std::pair<vec2ui, float>> residuals(allCollectedResidualsMap.begin(), allCollectedResidualsMap.end()); //std::sort(residuals.begin(), residuals.end(), [](const std::pair<vec2ui, float> &left, const std::pair<vec2ui, float> &right) { //debugging only // return left.second > right.second; //}); //if (m_maxResImPairs.size() > 1) { // std::ofstream s("debug/_logs/" + std::to_string(solverInput.numberOfImages) + "_" + std::to_string(m_maxResImPairs.front().x) + "-" + std::to_string(m_maxResImPairs.front().y) + ".txt"); // s << "# im pairs to remove = " << m_maxResImPairs.size() << ", res thresh = " << parameters.highResidualThresh << std::endl; // for (unsigned int i = 0; i < m_maxResImPairs.size(); i++) s << m_maxResImPairs[i] << std::endl; // s.close(); //} //!!!debugging } } #endif m_solverExtra.h_maxResidual[0] = maxResidual; m_solverExtra.h_maxResidualIndex[0] = maxResidualIndex; } else { m_solverExtra.h_maxResidual[0] = 0.0f; m_solverExtra.h_maxResidualIndex[0] = 0; } if (m_timer) m_timer->endEvent(); }
void TriangulatedPoint::connectTracks(std::vector<PointCorrespondence> const& corrs_, std::vector<TriangulatedPoint>& points, int nRequiredMeasurements) { points.clear(); if (corrs_.empty()) return; // Change the image correspondences, s.t. the left view has smaller id vector<PointCorrespondence> corrs(corrs_); for (unsigned i = 0; i < corrs.size(); ++i) { if (corrs[i].left.view > corrs[i].right.view) swap(corrs[i].left, corrs[i].right); } // Sort the correspondences, s.t. corresponding points with smaller left // view id come first sort(corrs.begin(), corrs.end(), CorrespondenceLess()); // for (size_t i = 0; i < corrs.size(); ++i) // { // PointCorrespondence const& c = corrs[i]; // cout << "left view = " << c.left.view << " left id = " << c.left.id // << " right view = " << c.right.view << " right id = " << c.right.id << endl; // } // map from tight index [0..nViews) -> camera id map<int, int> cameraIdMap; for (unsigned i = 0; i < corrs.size(); ++i) { // Skip measurements with no valid view (camera) id if (corrs[i].left.view < 0 || corrs[i].right.view < 0) continue; if (cameraIdMap.find(corrs[i].left.view) == cameraIdMap.end()) cameraIdMap.insert(make_pair(corrs[i].left.view, cameraIdMap.size())); if (cameraIdMap.find(corrs[i].right.view) == cameraIdMap.end()) cameraIdMap.insert(make_pair(corrs[i].right.view, cameraIdMap.size())); } unsigned const nViews = cameraIdMap.size(); // Maps view id -> vector of associated Links vector<vector<Link> > distributedLinks(nViews); for (unsigned i = 0; i < corrs.size(); ++i) { PointCorrespondence const& corr = corrs[i]; if (corr.left.view < 0 || corr.right.view < 0) continue; int l = cameraIdMap[corr.left.view]; int r = cameraIdMap[corr.right.view]; distributedLinks[l].push_back(Link(corr.left)); distributedLinks[r].push_back(Link(corr.right)); } vector<TriangulatedPoint> tmpPoints; vector<pair<int, int> > equivalences; for (unsigned i = 0; i < corrs.size(); ++i) { PointCorrespondence const& corr = corrs[i]; if (corr.left.id < 0 || corr.right.id < 0) continue; if (corrs[i].left.view < 0 || corrs[i].right.view < 0) continue; int l = cameraIdMap[corr.left.view]; int r = cameraIdMap[corr.right.view]; Link& left = findMatchingLink(corr.left, distributedLinks[l]); Link& right = findMatchingLink(corr.right, distributedLinks[r]); if (left.point < 0 && right.point < 0) { tmpPoints.push_back(TriangulatedPoint()); tmpPoints.back().measurements.push_back(corr.left); tmpPoints.back().measurements.push_back(corr.right); left.point = tmpPoints.size() - 1; right.point = left.point; } else if (left.point >= 0 && right.point < 0) { tmpPoints[left.point].measurements.push_back(corr.right); right.point = left.point; } else if (left.point < 0 && right.point >= 0) { tmpPoints[right.point].measurements.push_back(corr.left); left.point = right.point; } else { #if 1 // This case indicates a loop in the view configuration. // Register equivalent triangulated points for now. if (left.point != right.point) { if (left.point < right.point) equivalences.push_back(make_pair(left.point, right.point)); else equivalences.push_back(make_pair(right.point, left.point)); } #else VRException::vrerror("VRTriangulatedPoint::linkMeasurements()", __FILE__, __LINE__, "Internal error: need to unify two triangulated points!"); #endif } } // end for (i) // Handle registered equivalences. map<int, int> substitutions; for (unsigned i = 0; i < equivalences.size(); ++i) { pair<int, int> const& assoc = equivalences[i]; // Note: assoc.second > assoc.first. int target = assoc.first; map<int, int>::const_iterator p = substitutions.find(assoc.second); // Follow the chain of substitutions until the end while (p != substitutions.end()) { target = (*p).second; p = substitutions.find(target); } substitutions.insert(make_pair(assoc.second, target)); // Copy the image measurements to the target TriangulatedPoint TriangulatedPoint const& src = tmpPoints[assoc.second]; TriangulatedPoint& dest = tmpPoints[target]; for (unsigned j = 0; j < src.measurements.size(); ++j) dest.measurements.push_back(src.measurements[j]); } // end for (i) for (unsigned i = 0; i < tmpPoints.size(); ++i) { // Ignore all points which are substituted (unified) and points with // too few views. if (substitutions.find(i) == substitutions.end() && tmpPoints[i].measurements.size() >= nRequiredMeasurements) points.push_back(tmpPoints[i]); } } // end TriangulatedPoint::connectTracks()