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()
