int main(int argc, char *argv[]) {
// Initialize GLUT and open a window.
glutInit(&argc, argv);
glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE | GLUT_DEPTH);
glutInitWindowSize(800, 600);
glutCreateWindow(argv[0]);
// Register a bunch of callbacks for GLUT events.
glutDisplayFunc(display);
glutReshapeFunc(reshape);
glutMotionFunc(mouseMove);
glutMouseFunc(mouseClick);
glutKeyboardFunc(keyboard);
// Setup OpenGL
initOpenGL();
// Initialize our robot.
if (argc > 1) {
std::vector<Pose> poses;
readPoseFile(argv[1], poses);
robot.setPoses(poses);
}
// Schedule the first animation callback ASAP.
glutTimerFunc(0, animate, 0);
// Pass control to GLUT.
glutMainLoop();
// Will never be reached.
return 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();
}
