Esempio n. 1
0
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();
}
Esempio n. 4
0
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;
    }
}
Esempio n. 6
0
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));
}
Esempio n. 7
0
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);
}
Esempio n. 8
0
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;
}
Esempio n. 9
0
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;
}
Esempio n. 10
0
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();
}
Esempio n. 11
0
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;

}