Statistics
| Branch: | Revision:

root / rgbdslam / src / graph_manager.h @ 9240aaa3

History | View | Annotate | Download (7.18 KB)

1
/* This file is part of RGBDSLAM.
2
 * 
3
 * RGBDSLAM is free software: you can redistribute it and/or modify
4
 * it under the terms of the GNU General Public License as published by
5
 * the Free Software Foundation, either version 3 of the License, or
6
 * (at your option) any later version.
7
 * 
8
 * RGBDSLAM is distributed in the hope that it will be useful,
9
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
10
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
11
 * GNU General Public License for more details.
12
 * 
13
 * You should have received a copy of the GNU General Public License
14
 * along with RGBDSLAM.  If not, see <http://www.gnu.org/licenses/>.
15
 */
16

    
17

    
18
/*
19
 * graph_manager.h
20
 *
21
 *  Created on: 19.01.2011
22
 *      Author: hess
23
 */
24

    
25
#ifndef GRAPH_MANAGER_H_
26
#define GRAPH_MANAGER_H_
27

    
28
#include <ros/ros.h>
29
#include <tf/transform_broadcaster.h>
30
#include "node.h"
31
#include <pcl/filters/voxel_grid.h>
32
#include <Eigen/Core>
33
#include <Eigen/Geometry>
34
#include <opencv2/core/core.hpp>
35
#include <opencv2/features2d/features2d.hpp>
36
#include <map>
37
#include <QObject>
38
#include <QString>
39
#include <QMatrix4x4>
40
#include <QList>
41
#include <QMutex>
42

    
43
#include <iostream>
44
#include <sstream>
45
#include <string>
46
#include <ctime>
47
#include <memory> //for auto_ptr
48
#include "parameter_server.h"
49

    
50
#include "g2o/core/graph_optimizer_sparse.h"
51

    
52
#include "g2o/core/hyper_dijkstra.h"
53

    
54
//#define ROSCONSOLE_SEVERITY_INFO
55

    
56
//!Computes a globally optimal trajectory from transformations between Node-pairs
57
class GraphManager : public QObject {
58
    Q_OBJECT
59
    Q_SIGNALS:
60
    ///Connect to this signal to get the transformation matrix from the last frame as QString
61
    void newTransformationMatrix(QString);
62
    void sendFinished();
63
    void setGUIInfo(QString message);
64
    void setGUIStatus(QString message);
65
    void setPointCloud(pointcloud_type * pc, QMatrix4x4 transformation);
66
    void updateTransforms(QList<QMatrix4x4>* transformations);
67
    void setGUIInfo2(QString message);
68
    void setGraphEdges(QList<QPair<int, int> >* edge_list);
69
    void deleteLastNode();
70
    void resetGLViewer();
71
    
72
    public Q_SLOTS:
73
    /// Start over with new graph
74
    void reset();
75
    ///iterate over all Nodes, sending their transform and pointcloud
76
    void sendAllClouds();
77
    ///Call saveIndividualCloudsToFile, as background thread if threaded=true and possible 
78
    void saveIndividualClouds(QString file_basename, bool threaded=true);
79
    ///Call saveAllCloudsToFile,  as background thread if threaded=true and possible 
80
    void saveAllClouds(QString filename, bool threaded=true);
81
    ///Throw the last node out, reoptimize
82
    void deleteLastFrame(); 
83
    void setMaxDepth(float max_depth);
84
    ///Save trajectory of computed motion and ground truth (if available)
85
    void saveTrajectory(QString filebasename);
86
    void cloudRendered(pointcloud_type const * pc);
87
    void optimizeGraph(int iter = -1);
88
    void printEdgeErrors(QString);
89
    void pruneEdgesWithErrorAbove(float);
90
    void sanityCheck(float);
91

    
92
    public:
93
    GraphManager(ros::NodeHandle);
94
    ~GraphManager();
95

    
96
    /// Add new node to the graph.
97
    /// Node will be included, if a valid transformation to one of the former nodes
98
    /// can be found. If appropriate, the graph is optimized
99
    /// graphmanager owns newNode after this call. Do no delete the object
100
    bool addNode(Node* newNode); 
101

    
102
    ///Draw the features's motions onto the canvas
103
    void drawFeatureFlow(cv::Mat& canvas, 
104
                         cv::Scalar line_color = cv::Scalar(255,0,0,0), 
105
                         cv::Scalar circle_color = cv::Scalar(0,0,255,0)); 
106

    
107
    bool isBusy();
108
    
109
    float Max_Depth;
110
    //void setMaxDepth(float max_depth);
111
    //!Warning: This is a dangerous way to save memory. Some methods will behave undefined after this.
112
    ///Notable exception: optimizeGraph()
113
    void deleteFeatureInformation();
114
protected:
115
    
116
    ///iterate over all Nodes, transform them to the fixed frame, aggregate and save
117
    void saveAllCloudsToFile(QString filename);
118
    ///iterate over all Nodes, save each in one pcd-file
119
    void saveIndividualCloudsToFile(QString filename);
120
    void pointCloud2MeshFile(QString filename, pointcloud_type full_cloud);
121
    std::vector < cv::DMatch > last_inlier_matches_;
122
    std::vector < cv::DMatch > last_matches_;
123

    
124
    /// The parameter max_targets determines how many potential edges are wanted
125
    /// max_targets < 0: No limit
126
    /// max_targets = 0: Compare to first frame only
127
    /// max_targets = 1: Compare to previous frame only
128
    /// max_targets > 1: Select intelligently
129
    QList<int> getPotentialEdgeTargets(const Node* new_node, int max_targets);
130
    
131
    //std::vector<int> getPotentialEdgeTargetsFeatures(const Node* new_node, int max_targets);
132
    
133
    bool addEdgeToG2O(const LoadedEdge3D& edge, bool good_edge, bool set_estimate=false);
134

    
135

    
136
    ///Send markers to visualize the graph edges (cam transforms) in rviz (if somebody subscribed)
137
    void visualizeGraphEdges() const;
138
    ///Send markers to visualize the graph nodes (cam positions) in rviz (if somebody subscribed)
139
    void visualizeGraphNodes() const;
140
    ///Send markers to visualize the graph ids in rviz (if somebody subscribed)
141
    void visualizeGraphIds() const;
142
        
143
    
144
    
145
    ///Send markers to visualize the last matched features in rviz (if somebody subscribed)
146
    void visualizeFeatureFlow3D(unsigned int marker_id = 0,
147
                                bool draw_outlier = true) const;
148
    ///Send the transform between openni_camera (the initial position of the cam)
149
    ///and the cumulative motion. 
150
    ///This is called periodically by a ros timer and after each optimizer run
151
    void broadcastTransform(const ros::TimerEvent& event);
152
    ///Constructs a list of transformation matrices from all nodes (used for visualization in glviewer)
153
    ///The object lives on the heap and needs to be destroyed somewhere else (i.e. in glviewer)
154
    QList<QMatrix4x4>* getAllPosesAsMatrixList();
155
    QList<QPair<int, int> >* getGraphEdges() const;
156
    void resetGraph();
157

    
158
    void mergeAllClouds(pointcloud_type & merge);
159
    double geodesicDiscount(g2o::HyperDijkstra& hypdij, const MatchingResult& mr);
160
    
161
    g2o::SparseOptimizer* optimizer_;
162

    
163
    ros::Publisher marker_pub_; 
164
    ros::Publisher ransac_marker_pub_;
165
    ros::Publisher whole_cloud_pub_;
166
    ros::Publisher batch_cloud_pub_;
167
    
168
    //!Used to start the broadcasting of the pose estimate regularly
169
    ros::Timer timer_;
170
    //!Used to broadcast the pose estimate
171
    tf::TransformBroadcaster br_;
172
    tf::Transform computed_motion_; ///<transformation of the last frame to the first frame (assuming the first one is fixed)
173
    tf::Transform  init_base_pose_;
174
    tf::Transform base2points_;//base_frame -> optical_frame 
175
    //Eigen::Matrix4f latest_transform_;///<same as computed_motion_ as Eigen
176
    QMatrix4x4 latest_transform_;///<same as computed_motion_ as Eigen
177

    
178

    
179
    //!Map from node id to node. Assumption is, that ids start at 0 and are consecutive
180
    std::map<int, Node* > graph_;
181
    bool reset_request_;
182
    unsigned int marker_id;
183
    int last_matching_node_;
184
    g2o::SE3Quat last_edge_;
185
    bool batch_processing_runs_;
186
    bool process_node_runs_;
187
    QMutex optimizer_mutex;
188
    //cv::FlannBasedMatcher global_flann_matcher;
189
    
190
};
191

    
192

    
193

    
194
#endif /* GRAPH_MANAGER_H_ */