00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #ifndef RTABMAP_DATABASEVIEWER_H_
00029 #define RTABMAP_DATABASEVIEWER_H_
00030
00031 #include "rtabmap/gui/RtabmapGuiExp.h"
00032
00033 #include <QMainWindow>
00034 #include <QtCore/QByteArray>
00035 #include <QtCore/QMap>
00036 #include <QtCore/QSet>
00037 #include <QtGui/QImage>
00038 #include <opencv2/core/core.hpp>
00039 #include <opencv2/features2d/features2d.hpp>
00040 #include <set>
00041 #include <vector>
00042 #include <pcl/point_cloud.h>
00043 #include <pcl/point_types.h>
00044
00045 #include <rtabmap/core/Link.h>
00046 #include <rtabmap/core/Signature.h>
00047
00048 class Ui_DatabaseViewer;
00049 class QGraphicsScene;
00050 class QGraphicsView;
00051 class QLabel;
00052 class QDialog;
00053
00054 namespace rtabmap
00055 {
00056 class DBDriver;
00057 class ImageView;
00058 class SensorData;
00059 class CloudViewer;
00060 class OctoMap;
00061 class ExportCloudsDialog;
00062 class EditDepthArea;
00063
00064 class RTABMAPGUI_EXP DatabaseViewer : public QMainWindow
00065 {
00066 Q_OBJECT
00067
00068 public:
00069 DatabaseViewer(const QString & ini = QString(), QWidget * parent = 0);
00070 virtual ~DatabaseViewer();
00071 bool openDatabase(const QString & path);
00072 bool isSavedMaximized() const {return savedMaximized_;}
00073 void showCloseButton(bool visible = true);
00074
00075 protected:
00076 virtual void showEvent(QShowEvent* anEvent);
00077 virtual void moveEvent(QMoveEvent* anEvent);
00078 virtual void resizeEvent(QResizeEvent* anEvent);
00079 virtual void keyPressEvent(QKeyEvent *event);
00080 virtual void closeEvent(QCloseEvent* event);
00081 virtual bool eventFilter(QObject *obj, QEvent *event);
00082
00083 private Q_SLOTS:
00084 void writeSettings();
00085 void restoreDefaultSettings();
00086 void configModified();
00087 void openDatabase();
00088 bool closeDatabase();
00089 void recoverDatabase();
00090 void updateStatistics();
00091 void selectObstacleColor();
00092 void selectGroundColor();
00093 void selectEmptyColor();
00094 void editDepthImage();
00095 void generateGraph();
00096 void exportSaved2DMap();
00097 void import2DMap();
00098 void viewOptimizedMesh();
00099 void exportOptimizedMesh();
00100 void updateOptimizedMesh();
00101 void exportDatabase();
00102 void extractImages();
00103 void exportPosesRaw();
00104 void exportPosesRGBDSLAM();
00105 void exportPosesKITTI();
00106 void exportPosesTORO();
00107 void exportPosesG2O();
00108 void exportPosesKML();
00109 void exportGPS_TXT();
00110 void exportGPS_KML();
00111 void generateLocalGraph();
00112 void regenerateLocalMaps();
00113 void regenerateCurrentLocalMaps();
00114 void view3DMap();
00115 void generate3DMap();
00116 void detectMoreLoopClosures();
00117 void refineAllNeighborLinks();
00118 void refineAllLoopClosureLinks();
00119 void resetAllChanges();
00120 void sliderAValueChanged(int);
00121 void sliderBValueChanged(int);
00122 void sliderAMoved(int);
00123 void sliderBMoved(int);
00124 void update3dView();
00125 void sliderNeighborValueChanged(int);
00126 void sliderLoopValueChanged(int);
00127 void sliderIterationsValueChanged(int);
00128 void updateGrid();
00129 void updateOctomapView();
00130 void updateGraphView();
00131 void refineConstraint();
00132 void addConstraint();
00133 void resetConstraint();
00134 void rejectConstraint();
00135 void updateConstraintView();
00136 void updateLoggerLevel();
00137 void updateStereo();
00138 void notifyParametersChanged(const QStringList &);
00139 void setupMainLayout(bool vertical);
00140
00141 private:
00142 QString getIniFilePath() const;
00143 void readSettings();
00144
00145 void updateIds();
00146 void update(int value,
00147 QLabel * labelIndex,
00148 QLabel * labelParents,
00149 QLabel * labelChildren,
00150 QLabel * weight,
00151 QLabel * label,
00152 QLabel * stamp,
00153 rtabmap::ImageView * view,
00154 QLabel * labelId,
00155 QLabel * labelMapId,
00156 QLabel * labelPose,
00157 QLabel * labelVelocity,
00158 QLabel * labeCalib,
00159 QLabel * labelGps,
00160 bool updateConstraintView);
00161 void updateStereo(const SensorData * data);
00162 void updateWordsMatching();
00163 void updateConstraintView(
00164 const rtabmap::Link & link,
00165 bool updateImageSliders = true,
00166 const Signature & signatureFrom = Signature(0),
00167 const Signature & signatureTo = Signature(0));
00168 void updateConstraintButtons();
00169 Link findActiveLink(int from, int to);
00170 bool containsLink(
00171 std::multimap<int, Link> & links,
00172 int from,
00173 int to);
00174 std::multimap<int, rtabmap::Link> updateLinksWithModifications(
00175 const std::multimap<int, rtabmap::Link> & edgeConstraints);
00176 void updateLoopClosuresSlider(int from = 0, int to = 0);
00177 void refineConstraint(int from, int to, bool silent);
00178 bool addConstraint(int from, int to, bool silent);
00179 void exportPoses(int format);
00180 void exportGPS(int format);
00181
00182 private:
00183 Ui_DatabaseViewer * ui_;
00184 CloudViewer * constraintsViewer_;
00185 CloudViewer * cloudViewer_;
00186 CloudViewer * stereoViewer_;
00187 CloudViewer * occupancyGridViewer_;
00188 QList<int> ids_;
00189 std::map<int, int> mapIds_;
00190 std::map<int, int> weights_;
00191 std::map<int, std::vector<int> > wmStates_;
00192 QMap<int, int> idToIndex_;
00193 QList<rtabmap::Link> neighborLinks_;
00194 QList<rtabmap::Link> loopLinks_;
00195 rtabmap::DBDriver * dbDriver_;
00196 QString pathDatabase_;
00197 std::string databaseFileName_;
00198 std::list<std::map<int, rtabmap::Transform> > graphes_;
00199 std::multimap<int, rtabmap::Link> graphLinks_;
00200 std::map<int, rtabmap::Transform> odomPoses_;
00201 std::map<int, rtabmap::Transform> groundTruthPoses_;
00202 std::map<int, rtabmap::Transform> gpsPoses_;
00203 std::map<int, GPS> gpsValues_;
00204 std::multimap<int, rtabmap::Link> links_;
00205 std::multimap<int, rtabmap::Link> linksRefined_;
00206 std::multimap<int, rtabmap::Link> linksAdded_;
00207 std::multimap<int, rtabmap::Link> linksRemoved_;
00208 std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > localMaps_;
00209 std::map<int, std::pair<float, cv::Point3f> > localMapsInfo_;
00210 std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > generatedLocalMaps_;
00211 std::map<int, std::pair<float, cv::Point3f> > generatedLocalMapsInfo_;
00212 std::map<int, cv::Mat> modifiedDepthImages_;
00213 OctoMap * octomap_;
00214 ExportCloudsDialog * exportDialog_;
00215 QDialog * editDepthDialog_;
00216 EditDepthArea * editDepthArea_;
00217
00218 bool savedMaximized_;
00219 bool firstCall_;
00220 QString iniFilePath_;
00221
00222 bool useLastOptimizedGraphAsGuess_;
00223 std::map<int, Transform> lastOptimizedGraph_;
00224 };
00225
00226 }
00227
00228 #endif