void writeResults(const std::string &queryfile, const std::string &resultdir, const QueryResults &results) {
//cout << queryfile<<endl;
int start = queryfile.rfind('/') < 0 ? 0 : queryfile.rfind('/');
int end = queryfile.rfind('.');
std::string filename = queryfile.substr(start + 1,end - start-1);
QDir directory;
directory.mkpath(resultdir.c_str());
//cout << "start: " << start <<" end: " <<end << " filename: " << filename <<flush<<endl;
std::string root = resultdir;
std::string filedir = root.append("/");
std::string filepath = filedir.append(filename);
ofstream fout(filepath.c_str());
fout << filename<<endl;
for(uint i = 0; i < results.size(); i++) {
//2012 use is
//cout << "cmp: " << (results[i].imageName.find("Extended") == std::string::npos ? 1 : 0) <<" " <<results[i].imageName<<endl;
// if(results[i].imageName.find("Extended") == std::string::npos)
// {
// continue;
// }
//cout << results[i].imageName<<endl;
int st = results[i].imageName.rfind('/') < 0 ? 0 : results[i].imageName.rfind('/');
int ed = results[i].imageName.rfind("view");
std::string temp = results[i].imageName.substr(0, st);
st = temp.rfind('/') < 0 ? 0 : temp.rfind('/');
std::string name = temp.substr(st+1, ed - st -1);
//fout << results[i].imageName<<endl;
fout << name << " " <<results[i].ratio <<endl;
}
}
void queryLine(const vector<vector<Mat> > &features, const LBDDatabase &db) {
double truePositive = 0;
double falsePositive = 0;
double falseNegative = 0;
for(size_t i = 0; i < features.size(); ++i) {
QueryResults results;
db.query(features[i], results, 4);
bool correct = false;
for(int j = 0; j < results.size(); ++j) {
if(abs((int)results[j].Id - (int)i) <= 5) {
truePositive++;
correct = true;
break;
}
}
if(!correct){
printf("false detection: query image %lu\n", i);
cout << results << endl;
sleep(1);
falseNegative++;
}
//printf("query image %d done with %d\n", i, correct);
}
printf("recall rate: %lf\n", truePositive / (truePositive + falseNegative));
}
void queryLineFrame(vector<Frame> &frames, LBDDatabase &db, int resultSize = 5) {
string filename = "queryLineResult.yml";
cv::FileStorage resultFS(filename.c_str(), cv::FileStorage::WRITE);
if(!resultFS.isOpened()) throw string("Could not open file ") + filename;
resultFS << "result" << "["; //result
for(size_t i = 0; i < frames.size(); ++i) {
resultFS<<"{";
resultFS << "queryImage" << "{";
resultFS << "frameID" << frames[i].getID();
resultFS << "frameName" << frames[i].getImageName();
resultFS << "framePose" << frames[i].getFramePose();
resultFS << "}"; //query image
QueryResults results;
db.query(frames[i].getLineFeatureDescs(), results, resultSize);
scaleSortedResult(results, results.back().Score, results.front().Score);
resultFS << "queryResults" << "[";
for(size_t j = 0; j < results.size(); ++j) {
int id = (int)results[j].Id;
double score = (double)results[j].Score;
vector<double> resultPose;
assert(db.getPose(id, resultPose));
resultFS << "{";
resultFS << "frameID" << id;
resultFS << "score" << score;
resultFS << "pose" << resultPose;
resultFS << "}";
}
resultFS << "]";
resultFS << "}";
}
resultFS << "]"; //result
resultFS.release();
}
void GraphicEnd::checkLoopClosure()
{
cout<<"**************************************************"<<endl;
cout<<"checking loop closure!!!"<<endl;
cout<<"**************************************************"<<endl;
SparseOptimizer& opt = _pSLAMEnd->globalOptimizer;
//first extract currKF desp and keys
vector<cv::KeyPoint> keys;
vector<vector<float> > descrip;
loadFeatures(currKF.frame.rgb,descrip,keys);
//add currKF into database
db.add(descrip);
//query, first is the image itself
QueryResults ret;
db.query(descrip,ret,10);//only get 3 highest score frame;
//ransac check
for(int j=1;j<ret.size();++j)
{
//extract chose image feature
vector<cv::KeyPoint> keypoint;
vector<vector<float> > descriptor;
loadFeatures(keyframes[ret[ j ].Id].frame.rgb,descriptor,keypoint);
bool ransacResult=checkFundumental(keys,descrip,keypoint,descriptor);
//if pass the ransac check
if(ransacResult)
{
Eigen::Matrix4f H;
bool validTrans=calcTrans2(keyframes[ ret[ j ].Id ],currKF,H);
loop_fout<<currKF.frame_index<<" and "<<keyframes[ret[ j ].Id].frame_index<<" : "<<validTrans<<endl;
if(!validTrans || H==Eigen::Matrix4f::Identity())
continue;
Eigen::Isometry3d T(H.cast<double>());
T = T.inverse();
EdgeSE3* edge = new EdgeSE3();
edge->vertices() [0] = opt.vertex( keyframes[ret[ j ].Id].id );
edge->vertices() [1] = opt.vertex( currKF.id );
Matrix<double, 6,6> information = Matrix<double, 6, 6>::Identity();
information(0, 0) = information(2,2) = 100;
information(1,1) = 100;
information(3,3) = information(4,4) = information(5,5) = 100;
edge->setInformation( information );
edge->setMeasurement( T );
edge->setRobustKernel( _pSLAMEnd->robustKernel );
opt.addEdge( edge );
}
}
}
//scale results to [0.5, 1]
void scaleSortedResult(QueryResults &resultVec, double minScore, double maxScore, double newMin = 0.5, double newMax = 1) {
for(size_t i = 0; i < resultVec.size(); ++i) {
double scaledScore = resultVec[i].Score;
if(scaledScore <= minScore)
scaledScore = minScore;
else if(scaledScore >= maxScore)
scaledScore = maxScore;
scaledScore = (newMax - newMin) * (scaledScore - minScore) / (maxScore - minScore) + newMin;
resultVec[i].Score = scaledScore;
}
}
void GetAll(boost::ptr_vector<acl::Group>& groups)
{
groups.clear();
QueryResults results;
mongo::Query query;
boost::unique_future<bool> future;
TaskPtr task(new db::Select("groups", query, results, future));
Pool::Queue(task);
future.wait();
if (results.size() == 0) return;
for (auto& obj: results)
groups.push_back(bson::Group::Unserialize(obj));
}
acl::GroupID GetNewGroupID()
{
boost::lock_guard<boost::mutex> lock(getNewGroupIdMtx);
QueryResults results;
boost::unique_future<bool> future;
mongo::Query query;
query.sort("gid", 0);
TaskPtr task(new db::Select("groups", query, results, future, 1));
Pool::Queue(task);
future.wait();
if (results.size() == 0) return acl::GroupID(1);
int gid = results.back().getIntField("gid");
return acl::GroupID(++gid);
}
void cal_pre_recall(QueryResults &ret, vector<float> &precision, vector<float> &recall)
{
precision.clear();
recall.clear();
int object = ret[0].Id/(obj_img+1);
float score=0.0;
for (uint i=1;i<ret.size(); i++) //get rid of top 1
{
if ( (ret[i].Id/(obj_img+1)) == object)
score+=1.0;
precision.push_back(score/(float)i);
recall.push_back(score/obj_img);
if (score==obj_img)
break;
}
for(uint i=0; i<precision.size(); i++)
cout << "Presicion " << i<<" is: " << precision[i]
<<"; recall: " <<recall[i]<< endl;
cout << endl;
}
bool PhotoGeocoder::geocoding_pipeline(string photoPath, GPSCoords &lla){
//--- Extract Exif data
//---------------------
cv::Mat K;
extract_k_matrix(photoPath, ccdData, K);
//--- Extract Features & Descriptors
//----------------------------------
vector<Feature> features;
vector<Descriptor> descriptors;
ftk->extract_features(photoPath,features,descriptors);
//--- VTree Match
//---------------
logger << INDENT << "Quering vocabulary tree for similar documents..." << "\n";
QueryResults listDocuments = vtree.query(descriptors, MAX_DOCUMENTS);
for(int i = 0; i < MAX_DOCUMENTS && i < (int) listDocuments.size(); i++){
logger << INDENT << "Loading document " << i << "\n";
vector<Descriptor> documentDescs;
vector<Point3D> points3D;
string pathDoc;
dbm.query_synth_path(listDocuments[i].Id, pathDoc);
//--- Load documents
//------------------
if(!load_synthetic(pathDoc, points3D, documentDescs))
return EXIT_FAILURE;
//--- Match photo<->documents
//---------------------------
logger << INDENT << "Matching with document... " << "\n";
vector<pair<int,int> > matches;
int nMatches = ftk->match_features(descriptors, documentDescs, matches);
logger << INDENT << nMatches << " sift matches were found" << "\n";
//--- If not enough matches
if(nMatches < M_LIMIT){
logger << INDENT << "not enough matches...skipping." << "\n";
continue;
}
vector<cv::Point2f> matchedPoints2D = vector<cv::Point2f>(nMatches);
vector<cv::Point3f> matchedPoints3D = vector<cv::Point3f>(nMatches);
for(int j = 0; j < nMatches; j++){
matchedPoints2D[j] = features[matches[j].first].pt;
matchedPoints3D[j] = points3D[matches[j].second];
}
//--- Compute Projection
//----------------------
cv::Matx34f P;
if(!find_proj_matrix(matchedPoints2D, matchedPoints3D,K,P)){
logger << INDENT << "Fail to compute P...skipping." << "\n";
continue;
}
cv::Matx44f G;
float error;
load_transformation_mat(strip_path(pathDoc) + "/../" + "transformMat.txt", error, G);
//--- Apply GPS transformation
//----------------------------
lla = apply_transformation(P, G);
//--- Compute Error
//-----------------
//TODO: this
//distance_gps(lla1,lla2);
logger << INDENT << photoPath << " --> geocoded at:" << "\n";
logger << INDENT << INDENT << "Latitude: " << lla.lat << "\n";
logger << INDENT << INDENT << "Longitude: " << lla.lon << "\n";
return true;
}
logger << INDENT << photoPath << " --> not geocoded" << "\n";
return false;
}
void loopClosing3d(const BoWFeatures &features)
{
int OFFSET = I_OFFSET;
double SANITY_THRESHOLD = SANITY;
double MATCH_THRESHOLD = SOGLIA_3D;
int SIZE_BUCKET = 7;
int TEMP_CONS = 5;
bool loop_found = false;
if (!DEBUG) { wait(); }
vector<int> bucket;
vector<double> xs, ys;
readPoseFile((dataset_sub+"syncPCDGT.txt").c_str(), xs, ys);
cout << "3D: Acquisizione Ground Truth" << endl;
ofstream report("report_3d.rep");
DUtilsCV::GUI::tWinHandler winplot = "Traiettoria3D";
DUtilsCV::Drawing::Plot::Style normal_style(2); // thickness
DUtilsCV::Drawing::Plot::Style loop_style('r', 2); // color, thickness
DUtilsCV::Drawing::Plot implot(240, 320,
- *std::max_element(xs.begin(), xs.end()),
- *std::min_element(xs.begin(), xs.end()),
*std::min_element(ys.begin(), ys.end()),
*std::max_element(ys.begin(), ys.end()), 25);
NarfVocabulary voc(filename_voc_3d);
NarfDatabase db(voc, false);
for (int i=0;i<files_list_3d.size();i++){
loop_found = false;
if ((i+1)>OFFSET){
BowVector v1, v2;
voc.transform(features[i], v1);
voc.transform(features[i-1], v2);
double san = voc.score(v1,v2);
cout << "S:" << san << endl;
if (san >= SANITY_THRESHOLD){
QueryResults ret;
db.query(features[i], ret,db.size());
for (int j = 0; j < ret.size(); j++){ //scansiono risultati
if (ret[j].Score > MATCH_THRESHOLD){ //sanity check
if ((i - OFFSET) >= ret[j].Id){ //scarto la coda
if (bucket.size() < SIZE_BUCKET){
bucket.push_back(ret[j].Id);
cout << i <<"-> bucket: " << ret[j].Id<<endl;
}
else
{
break;
}
}
}
}
ret.clear();
if (bucket.size()>0){
cout << "---- TEMPORAL CONSISTENCY {"<<i<< "} ----" << endl;
vector<int> i_back; //contiene le precedenti
for(int yyy = 0; yyy < TEMP_CONS; yyy++){
i_back.push_back((i-(yyy+1)));
}
BowVector v1, v2;
for (int aa = 0 ; aa < bucket.size(); aa++){//per ogni elemento del bucket
double max_score = 0;
int max_id = -1;
int centro = -1;
for (int cc = 0; cc<i_back.size();cc++){
voc.transform(features[i - (cc+1)],v1);
if (centro == -1){ centro = bucket[aa]; }
for(int bb = (centro - (TEMP_CONS/2) - 1) ; bb < centro + (TEMP_CONS/2); bb++){
int cursore = bb + 1;
if (cursore < 0) {continue;}
voc.transform(features[cursore],v2);
double score = voc.score(v1,v2);
cout << i - (cc+1) << " vs " << cursore << " score: " << score << endl;
if (score > MATCH_THRESHOLD && score > max_score){
max_id = cursore;
max_score = score;
}
}
if (max_id == -1){
bucket[aa] = -1;
centro = -1;
max_score = 0;
}else{
centro = max_id;
max_score = 0;
max_id = -1;
}
}
}
}
bucket.erase(std::remove( bucket.begin(), bucket.end(),-1), bucket.end());
double maxInliers = numeric_limits<double>::max();
double maxIdInliers = -1;
double tyu = 0;
if(bucket.size() == 1){
maxIdInliers = bucket[0];
}else{
#pragma omp parallel for
for(int yy = 0; yy < bucket.size(); yy++){
BowVector v1,v2;
tyu = reg_3D->getCentroidDiff(i,bucket[yy]);
cout << "score: " << tyu <<endl;
if (maxInliers > tyu){
maxInliers = tyu;
maxIdInliers = bucket[yy];
}
}
}
if(maxIdInliers != -1){
loop_found = true;
cout << "LOOP TROVATO : " << i <<" per immagine: " << maxIdInliers << endl;
implot.line(-xs[maxIdInliers], ys[maxIdInliers], -xs[i], ys[i], loop_style);
}
bucket.clear();
if(loop_found)
{
string ii,maxid;
ii = boost::lexical_cast<string>(i);
maxid = boost::lexical_cast<string>(maxIdInliers);
report << i <<":"<< maxIdInliers<<endl;
report.flush();
}
}
}
db.add(features[i]);
implot.line(-xs[i-1], ys[i-1], -xs[i], ys[i], normal_style);
DUtilsCV::GUI::showImage(implot.getImage(), true, &winplot, 10);
}
report.close();
stats* owl = new stats("report_3d.rep",registro_interno,OFFSET,"3D");
cout << owl->toString();
}
int PoseGraph::detectLoop(KeyFrame* keyframe, int frame_index)
{
// put image into image_pool; for visualization
cv::Mat compressed_image;
if (DEBUG_IMAGE)
{
int feature_num = keyframe->keypoints.size();
cv::resize(keyframe->image, compressed_image, cv::Size(376, 240));
putText(compressed_image, "feature_num:" + to_string(feature_num), cv::Point2f(10, 10), CV_FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255));
image_pool[frame_index] = compressed_image;
}
TicToc tmp_t;
//first query; then add this frame into database!
QueryResults ret;
TicToc t_query;
db.query(keyframe->brief_descriptors, ret, 4, frame_index - 50);
//printf("query time: %f", t_query.toc());
//cout << "Searching for Image " << frame_index << ". " << ret << endl;
TicToc t_add;
db.add(keyframe->brief_descriptors);
//printf("add feature time: %f", t_add.toc());
// ret[0] is the nearest neighbour's score. threshold change with neighour score
bool find_loop = false;
cv::Mat loop_result;
if (DEBUG_IMAGE)
{
loop_result = compressed_image.clone();
if (ret.size() > 0)
putText(loop_result, "neighbour score:" + to_string(ret[0].Score), cv::Point2f(10, 50), CV_FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255));
}
// visual loop result
if (DEBUG_IMAGE)
{
for (unsigned int i = 0; i < ret.size(); i++)
{
int tmp_index = ret[i].Id;
auto it = image_pool.find(tmp_index);
cv::Mat tmp_image = (it->second).clone();
putText(tmp_image, "index: " + to_string(tmp_index) + "loop score:" + to_string(ret[i].Score), cv::Point2f(10, 50), CV_FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255));
cv::hconcat(loop_result, tmp_image, loop_result);
}
}
// a good match with its nerghbour
if (ret.size() >= 1 &&ret[0].Score > 0.05)
for (unsigned int i = 1; i < ret.size(); i++)
{
//if (ret[i].Score > ret[0].Score * 0.3)
if (ret[i].Score > 0.015)
{
find_loop = true;
int tmp_index = ret[i].Id;
if (DEBUG_IMAGE && 0)
{
auto it = image_pool.find(tmp_index);
cv::Mat tmp_image = (it->second).clone();
putText(tmp_image, "loop score:" + to_string(ret[i].Score), cv::Point2f(10, 50), CV_FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255));
cv::hconcat(loop_result, tmp_image, loop_result);
}
}
}
/*
if (DEBUG_IMAGE)
{
cv::imshow("loop_result", loop_result);
cv::waitKey(20);
}
*/
if (find_loop && frame_index > 50)
{
int min_index = -1;
for (unsigned int i = 0; i < ret.size(); i++)
{
if (min_index == -1 || (ret[i].Id < min_index && ret[i].Score > 0.015))
min_index = ret[i].Id;
}
return min_index;
}
else
return -1;
}