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);
}
}
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;
}
