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_ */ |