//* Localization interface test *

#include "nav_map_match_markov.h" #include <fstream> #include <iostream> #include <iomanip> int main(){ NAV_Map_Match GLOBAL_MAP(100); cout << "Reading the global map. . ." << endl; // read the map with no markers on it except "all robots" trails URAMMMMMpppppppp if (! GLOBAL_MAP.read("m_global.ppm", 0x00)) { // -^^^^^^^-------- can be masked cout << "Cannot read the global map. Exiting. . ." << endl; exit(-1); } NAV_Map_Match LOCAL_MAP(100); cout << "Reading the local map. . ." << endl; if (! LOCAL_MAP.read("m_local.ppm", 0x00)) { cout << "Cannot read the local map. Exiting. . ." << endl; exit(-1); } // ArPose best_fit_pose; // NAV_Map_Match_origin_adjust(GLOBAL_MAP, LOCAL_MAP, best_fit_pose); cout << "Computing Markov Probabilities - will take forever. . ." << endl; NAV_Map_Base<ScoredHeading> MARKOV( GLOBAL_MAP.getResolution() ); NAV_Map_Match_origin_search(GLOBAL_MAP, LOCAL_MAP, MARKOV); { ofstream results("m_result.csv"); double xmax = MARKOV.getMaxX(); double dx = MARKOV.getResolution(); double ymax = MARKOV.getMaxY(); double dy = MARKOV.getResolution(); for (double y=MARKOV.getMinY()+dy; y<ymax; y=y+dy) results<<","<<y; results << std::endl; for (double x=MARKOV.getMinX()+dx; x<xmax; x=x+dx) { results<<x; for (double y=MARKOV.getMinY()+dy; y<ymax; y=y+dy) { results<<","<<MARKOV(x,y).score; } results << std::endl; } results << std::endl; results << std::endl; for (double y=MARKOV.getMinY()+dy; y<ymax; y=y+dy) results<<","<<y; results << std::endl; for (double x=MARKOV.getMinX()+dx; x<xmax; x=x+dx) { results<<x; for (double y=MARKOV.getMinY()+dy; y<ymax; y=y+dy) { results<<","<<MARKOV(x,y).pose.getTh(); } results << std::endl; } results.close(); } return(0); }