map-builder/main.cpp at master · piotrgomola/map-builder · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102

#include <iostream>
#include "main.h"
#include "Map/octomap.h"
#include "Map/gaussmap.h"
#include "Visualizer/Qvisualizer.h"
#include <GL/glut.h>
#include <qapplication.h>
#include "include/Utilities/img2pcl.h"

using namespace std;

int MAP_SIZE;
double res;
double raytraceFactor;

unsigned int filesSaved = 0;
bool saveCloud(false), noColor(false);

int main(int argc, char** argv) {

    tinyxml2::XMLDocument config;
    config.LoadFile("../../resources/config.xml");
    int mode, mapType;
    config.FirstChildElement("MapMode")->QueryIntText(&mode);
    config.FirstChildElement("MapType")->QueryIntText(&mapType);
    std::cout << "Start\n";

    if(mode == 0) {
        config.FirstChildElement("MapSize")->QueryIntText(&MAP_SIZE);
        config.FirstChildElement("MapRes")->QueryDoubleText(&res);
        config.FirstChildElement("RaytraceFactor")->QueryDoubleText(&raytraceFactor);
    } else {
        std::string mapSettingsPath = config.FirstChildElement( "ReadMapPath" )->GetText();
        mapSettingsPath += ".xml";
        tinyxml2::XMLDocument mapSettings;
        mapSettings.LoadFile(mapSettingsPath.c_str());
        mapSettings.FirstChildElement("MapSize")->QueryIntText(&MAP_SIZE);
        mapSettings.FirstChildElement("MapRes")->QueryDoubleText(&res);
        mapSettings.FirstChildElement("RaytraceFactor")->QueryDoubleText(&raytraceFactor);
    }

    int a=0;
    mapping::GrabbedImage PC;
    mapping::img2pcl img2pcl("../../resources/img2pcl.xml");

    QApplication application(argc,argv);
    setlocale(LC_NUMERIC,"C");
    glutInit(&argc, argv);

    QGLVisualizer visu;
    visu.setWindowTitle("Simulator viewer");
    visu.show();

    mapping::Map* map;
    if(mode == 0) {
        if(mapType != 0) {
            map = mapping::createMapGauss(PC.pointCloud);
        } else {
            map = mapping::createMapOcto(PC.pointCloud);
        }
        map->attachVisualizer(&visu);
    } else {
        std::string path = config.FirstChildElement( "ReadMapPath" )->GetText();
        map = mapping::createMapGauss(path);
        map->attachVisualizer(&visu);
    }

    if(mode == 0) {

        int calcEvery, calcUntil;
        config.FirstChildElement("CalculateEvery")->QueryIntText(&calcEvery);
        config.FirstChildElement("CalculateUntil")->QueryIntText(&calcUntil);

        while(img2pcl.grabFrame()) {
            if(a%calcEvery == 0) {
                img2pcl.calcPCL();
                PC = img2pcl.returnPC();

                if(a == calcUntil) {
                    map->insertCloud(PC, true);
                    break;
                } else {
                    map->insertCloud(PC, false);
                }

            }
            a++;
        }
        int shouldSave;
        config.FirstChildElement("SaveMap")->QueryIntText(&shouldSave);
        if(shouldSave != 0) {
            map->saveMap();
        }
    } else {
        map->mapLoaded();
    }

    application.exec();

    std::cout << "Done\n";
}