Ejemplo n.º 1
0
void ofxIcp::find_closest_points(const int & how_many, const vector<cv::Point3d> & input_1, const vector<cv::Point3d> & input_2, vector<cv::Point3d> & output_1, vector<cv::Point3d> & output_2){
    
    
    ofxNearestNeighbour3D tree;
    vector<ofPoint> of_Input_2;
    toOF(input_2, of_Input_2);
    tree.buildIndex(of_Input_2);
    
    vector<closest_pair> pairs;
    
    /*
     //RANDOM
     vector<cv::Point3d> input_1_tmp = input_1;
     random_unique(input_1_tmp.begin(), input_1_tmp.end(), how_many);
     for(int i = 0; i < how_many; i++){
     vector<float> distsSq;
     vector<NNIndex> indicesKNN;
     ofPoint p_tmp(input_1_tmp[i].x, input_1_tmp[i].y, input_1_tmp[i].z);
     tree.findNClosestPoints(p_tmp, 1, indicesKNN, distsSq);
     
     closest_pair pair;
     pair.p1 = input_1_tmp[i];
     pair.p2 = input_2[indicesKNN[0]];
     pair.dist = distsSq[0];
     
     pairs.push_back(pair);
     }*/
    
    //UNIFORM
    int increment = input_1.size()/how_many;
    for(int i = 0; i < input_1.size(); i += increment){
        vector<float> distsSq;
        vector<NNIndex> indicesKNN;
        ofPoint p_tmp(input_1[i].x, input_1[i].y, input_1[i].z);
        tree.findNClosestPoints(p_tmp, 1, indicesKNN, distsSq);
        
        closest_pair pair;
        pair.p1 = input_1[i];
        pair.p2 = input_2[indicesKNN[0]];
        pair.dist = distsSq[0];
        
        pairs.push_back(pair);
    }
    
    sort(pairs.begin(), pairs.end(), closest_pair_sort);
    
    
    for(int i = 0; i < pairs.size() * 0.8; i++){
        output_1.push_back(pairs[i].p1);
        output_2.push_back(pairs[i].p2);
    }
}
Ejemplo n.º 2
0
TrackerSim::ptr_t TrackerSim::create(sim_t sim, unsigned int read_interval_ms, std::string filename)
{
	ptr_t p_tmp(new TrackerSim(sim, read_interval_ms, filename));
	return p_tmp;
}