root / rgbdslam / src / graph_manager.cpp @ master
History | View | Annotate | Download (61.8 KB)
1 | 9240aaa3 | Alex | /* 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 | #include <sys/time.h> |
||
19 | #include <visualization_msgs/Marker.h> |
||
20 | #include <geometry_msgs/Point.h> |
||
21 | //#include <rgbdslam/CloudTransforms.h>
|
||
22 | #include "graph_manager.h" |
||
23 | #include "misc.h" |
||
24 | #include "pcl_ros/transforms.h" |
||
25 | #include "pcl/io/pcd_io.h" |
||
26 | #include <sensor_msgs/PointCloud2.h> |
||
27 | #include <opencv2/features2d/features2d.hpp> |
||
28 | #include <QThread> |
||
29 | #include <qtconcurrentrun.h> |
||
30 | #include <QtConcurrentMap> |
||
31 | #include <QFile> |
||
32 | #include <utility> |
||
33 | #include <fstream> |
||
34 | |||
35 | #include "g2o/math_groups/se3quat.h" |
||
36 | #include "g2o/types/slam3d/edge_se3_quat.h" |
||
37 | |||
38 | #include "g2o/core/block_solver.h" |
||
39 | #include "g2o/solvers/csparse/linear_solver_csparse.h" |
||
40 | #include "g2o/solvers/cholmod/linear_solver_cholmod.h" |
||
41 | //#include "g2o/solvers/pcg/linear_solver_pcg.h"
|
||
42 | |||
43 | //typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> > SlamBlockSolver;
|
||
44 | typedef g2o::BlockSolver< g2o::BlockSolverTraits<6, 3> > SlamBlockSolver; |
||
45 | typedef g2o::LinearSolverCSparse<SlamBlockSolver::PoseMatrixType> SlamLinearSolver;
|
||
46 | typedef g2o::LinearSolverCholmod<SlamBlockSolver::PoseMatrixType> SlamLinearCholmodSolver;
|
||
47 | //typedef g2o::LinearSolverPCG<SlamBlockSolver::PoseMatrixType> SlamLinearPCGSolver;
|
||
48 | //typedef std::map<int, g2o::VertexSE3*> VertexIDMap;
|
||
49 | typedef std::tr1::unordered_map<int, g2o::HyperGraph::Vertex*> VertexIDMap; |
||
50 | //std::tr1::unordered_map<int, g2o::HyperGraph::Vertex* >
|
||
51 | typedef std::set<g2o::HyperGraph::Edge*> EdgeSet;
|
||
52 | |||
53 | |||
54 | GraphManager::GraphManager(ros::NodeHandle nh) : |
||
55 | optimizer_(0),
|
||
56 | latest_transform_(), //constructs identity
|
||
57 | reset_request_(false),
|
||
58 | marker_id(0),
|
||
59 | last_matching_node_(-1),
|
||
60 | batch_processing_runs_(false),
|
||
61 | process_node_runs_(false)
|
||
62 | { |
||
63 | struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime); |
||
64 | |||
65 | // allocating the optimizer
|
||
66 | optimizer_ = new g2o::SparseOptimizer();
|
||
67 | optimizer_->setVerbose(true);
|
||
68 | //SlamLinearSolver* linearSolver = new SlamLinearSolver();
|
||
69 | SlamLinearCholmodSolver* linearSolver = new SlamLinearCholmodSolver();
|
||
70 | //SlamLinearPCGSolver* linearSolver = new SlamLinearPCGSolver();
|
||
71 | linearSolver->setBlockOrdering(false);
|
||
72 | SlamBlockSolver* solver = new SlamBlockSolver(optimizer_, linearSolver);
|
||
73 | optimizer_->setSolver(solver); |
||
74 | |||
75 | //optimizer_ = new AIS::HCholOptimizer3D(numLevels, nodeDistance);
|
||
76 | ParameterServer* ps = ParameterServer::instance(); |
||
77 | batch_cloud_pub_ = nh.advertise<sensor_msgs::PointCloud2>(ps->get<std::string>("individual_cloud_out_topic"), |
||
78 | ps->get<int>("publisher_queue_size")); |
||
79 | whole_cloud_pub_ = nh.advertise<sensor_msgs::PointCloud2>(ps->get<std::string>("aggregate_cloud_out_topic"), |
||
80 | ps->get<int>("publisher_queue_size")); |
||
81 | ransac_marker_pub_ = nh.advertise<visualization_msgs::Marker>("/rgbdslam/correspondence_marker",
|
||
82 | ps->get<int>("publisher_queue_size")); |
||
83 | marker_pub_ = nh.advertise<visualization_msgs::Marker>("/rgbdslam/pose_graph_markers",
|
||
84 | ps->get<int>("publisher_queue_size")); |
||
85 | computed_motion_ = tf::Transform::getIdentity(); |
||
86 | init_base_pose_ = tf::Transform::getIdentity(); |
||
87 | base2points_ = tf::Transform::getIdentity(); |
||
88 | timer_ = nh.createTimer(ros::Duration(0.1), &GraphManager::broadcastTransform, this); |
||
89 | |||
90 | Max_Depth = -1;
|
||
91 | |||
92 | clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s"); |
||
93 | } |
||
94 | |||
95 | |||
96 | //WARNING: Dangerous
|
||
97 | void GraphManager::deleteFeatureInformation() {
|
||
98 | ROS_WARN("Clearing out Feature information from nodes");
|
||
99 | for (unsigned int i = 0; i < graph_.size(); ++i) { |
||
100 | graph_[i]->clearFeatureInformation(); |
||
101 | } |
||
102 | } |
||
103 | |||
104 | GraphManager::~GraphManager() { |
||
105 | //TODO: delete all Nodes
|
||
106 | //for (unsigned int i = 0; i < optimizer_->vertices().size(); ++i) {
|
||
107 | for (unsigned int i = 0; i < graph_.size(); ++i) { |
||
108 | Node* to_delete = graph_[i]; |
||
109 | delete to_delete;
|
||
110 | } |
||
111 | graph_.clear(); |
||
112 | delete (optimizer_);
|
||
113 | ransac_marker_pub_.shutdown(); |
||
114 | whole_cloud_pub_.shutdown(); |
||
115 | marker_pub_.shutdown(); |
||
116 | batch_cloud_pub_.shutdown(); |
||
117 | |||
118 | } |
||
119 | |||
120 | void GraphManager::drawFeatureFlow(cv::Mat& canvas, cv::Scalar line_color,
|
||
121 | cv::Scalar circle_color){ |
||
122 | struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime); |
||
123 | if(!ParameterServer::instance()->get<bool>("use_gui")){ return; } |
||
124 | ROS_DEBUG("Number of features to draw: %d", (int)last_inlier_matches_.size()); |
||
125 | |||
126 | const double pi_fourth = 3.14159265358979323846 / 4.0; |
||
127 | const int line_thickness = 1; |
||
128 | const int circle_radius = 6; |
||
129 | if(graph_.size() == 0) { |
||
130 | ROS_WARN("Feature Flow for empty graph requested. Bug?");
|
||
131 | return;
|
||
132 | } else if(graph_.size() == 1 || last_matching_node_ == -1 ) {//feature flow is only available between at least two nodes |
||
133 | Node* newernode = graph_[graph_.size()-1];
|
||
134 | cv::drawKeypoints(canvas, newernode->feature_locations_2d_, canvas, cv::Scalar(255), 5); |
||
135 | return;
|
||
136 | } |
||
137 | |||
138 | Node* earliernode = graph_[last_matching_node_];//graph_.size()-2; //compare current to previous
|
||
139 | Node* newernode = graph_[graph_.size()-1];
|
||
140 | if(earliernode == NULL){ |
||
141 | if(newernode == NULL ){ ROS_ERROR("Nullpointer for Node %u", (unsigned int)graph_.size()-1); } |
||
142 | ROS_ERROR("Nullpointer for Node %d", last_matching_node_);
|
||
143 | last_matching_node_ = 0;
|
||
144 | return;
|
||
145 | } else if(newernode == NULL ){ |
||
146 | ROS_ERROR("Nullpointer for Node %u", (unsigned int)graph_.size()-1); |
||
147 | return;
|
||
148 | } |
||
149 | |||
150 | //encircle all keypoints in this image
|
||
151 | //for(unsigned int feat = 0; feat < newernode->feature_locations_2d_.size(); feat++) {
|
||
152 | // cv::Point2f p;
|
||
153 | // p = newernode->feature_locations_2d_[feat].pt;
|
||
154 | // cv::circle(canvas, p, circle_radius, circle_color, line_thickness, 8);
|
||
155 | //}
|
||
156 | cv::Mat tmpimage = cv::Mat::zeros(canvas.rows, canvas.cols, CV_8UC1); |
||
157 | cv::drawKeypoints(canvas, newernode->feature_locations_2d_, tmpimage, cv::Scalar(155), 5); |
||
158 | canvas+=tmpimage; |
||
159 | for(unsigned int mtch = 0; mtch < last_inlier_matches_.size(); mtch++) { |
||
160 | cv::Point2f p,q; //TODO: Use sub-pixel-accuracy
|
||
161 | unsigned int newer_idx = last_inlier_matches_[mtch].queryIdx; |
||
162 | unsigned int earlier_idx = last_inlier_matches_[mtch].trainIdx; |
||
163 | q = newernode->feature_locations_2d_[newer_idx].pt; |
||
164 | p = earliernode->feature_locations_2d_[earlier_idx].pt; |
||
165 | |||
166 | double angle; angle = atan2( (double) p.y - q.y, (double) p.x - q.x ); |
||
167 | double hypotenuse = cv::norm(p-q);
|
||
168 | cv::line(canvas, p, q, line_color, line_thickness, 8);
|
||
169 | if(hypotenuse > 1.5){ //only larger motions larger than one pix get an arrow tip |
||
170 | cv::line( canvas, p, q, line_color, line_thickness, 8 );
|
||
171 | /* Now draw the tips of the arrow. */
|
||
172 | p.x = (q.x + 4 * cos(angle + pi_fourth));
|
||
173 | p.y = (q.y + 4 * sin(angle + pi_fourth));
|
||
174 | cv::line( canvas, p, q, line_color, line_thickness, 8 );
|
||
175 | p.x = (q.x + 4 * cos(angle - pi_fourth));
|
||
176 | p.y = (q.y + 4 * sin(angle - pi_fourth));
|
||
177 | cv::line( canvas, p, q, line_color, line_thickness, 8 );
|
||
178 | } else { //draw a smaller circle into the bigger one |
||
179 | cv::circle(canvas, p, circle_radius-2, circle_color, line_thickness, 8); |
||
180 | } |
||
181 | } |
||
182 | clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s"); |
||
183 | } |
||
184 | |||
185 | |||
186 | /// max_targets determines how many potential edges are wanted
|
||
187 | /// max_targets < 0: No limit
|
||
188 | /// max_targets = 0: Compare to first frame only
|
||
189 | /// max_targets = 1: Compare to previous frame only
|
||
190 | /// max_targets > 1: Select intelligently (TODO: rather stupid at the moment)
|
||
191 | QList<int> GraphManager::getPotentialEdgeTargets(const Node* new_node, int max_targets){ |
||
192 | int last_targets = 3; //always compare to the last n, spread evenly for the rest |
||
193 | QList<int> ids_to_link_to;
|
||
194 | //max_targets = last_targets;
|
||
195 | int gsize = graph_.size();
|
||
196 | //Special Cases
|
||
197 | if(gsize == 0){ |
||
198 | ROS_WARN("Do not call this function as long as the graph is empty");
|
||
199 | return ids_to_link_to;
|
||
200 | } |
||
201 | if(max_targets-last_targets < 0) |
||
202 | return ids_to_link_to;
|
||
203 | if(max_targets-last_targets == 0){ |
||
204 | ids_to_link_to.push_back(0);
|
||
205 | return ids_to_link_to; //only compare to first frame |
||
206 | } else if(max_targets-last_targets == 1){ |
||
207 | ids_to_link_to.push_back(gsize-2);
|
||
208 | return ids_to_link_to; //only compare to previous frame |
||
209 | } |
||
210 | //End Special Cases
|
||
211 | |||
212 | //All the last few nodes
|
||
213 | if(gsize <= max_targets){
|
||
214 | last_targets = gsize; |
||
215 | } |
||
216 | for(int i = 2; i <= gsize && i <= last_targets; i++){//start at two, b/c the prev node is always already checked in addNode{ |
||
217 | ids_to_link_to.push_back(gsize-i); |
||
218 | } |
||
219 | std::cout << " ids_to_link_to.size: " << ids_to_link_to.size();
|
||
220 | while(ids_to_link_to.size() < max_targets && ids_to_link_to.size() < gsize-1){ |
||
221 | int sample_id = rand() % (gsize - 1); |
||
222 | ROS_DEBUG_STREAM("Sample: " << sample_id << " Graph size: " << gsize << " ids_to_link_to.size: " << ids_to_link_to.size()); |
||
223 | //usleep(100000);
|
||
224 | QList<int>::const_iterator i1 = qFind(ids_to_link_to, sample_id);
|
||
225 | if(i1 != ids_to_link_to.end())
|
||
226 | continue;
|
||
227 | ids_to_link_to.push_back(sample_id); |
||
228 | } |
||
229 | |||
230 | |||
231 | //output only loop
|
||
232 | std::stringstream ss; |
||
233 | ss << "Node ID's to compare with candidate for node " << graph_.size() << ":"; |
||
234 | for(int i = 0; i < (int)ids_to_link_to.size()-1; i++){ |
||
235 | ss << ids_to_link_to[i] << ", " ;
|
||
236 | } |
||
237 | ROS_INFO("%s%i", ss.str().c_str(), ids_to_link_to.last()); //print last one here, to avoid trailing "," |
||
238 | return ids_to_link_to;
|
||
239 | } |
||
240 | ///// max_targets determines how many potential edges are wanted
|
||
241 | ///// max_targets < 0: No limit
|
||
242 | ///// max_targets = 0: Compare to first frame only
|
||
243 | ///// max_targets = 1: Compare to previous frame only
|
||
244 | ///// max_targets > 1: Select intelligently (TODO: rather stupid at the moment)
|
||
245 | //QList<int> GraphManager::getPotentialEdgeTargets(const Node* new_node, int max_targets){
|
||
246 | // const int last_targets = 3; //always compare to the last n, spread evenly for the rest
|
||
247 | // QList<int> ids_to_link_to;
|
||
248 | // double max_id_plus1 = (double)graph_.size()- last_targets;
|
||
249 | // max_targets -= last_targets;
|
||
250 | // //Special Cases
|
||
251 | // if(graph_.size() == 0){
|
||
252 | // ROS_WARN("Do not call this function as long as the graph is empty");
|
||
253 | // return ids_to_link_to;
|
||
254 | // }
|
||
255 | // if(max_targets < 0)
|
||
256 | // return ids_to_link_to;
|
||
257 | // if(max_targets == 0){
|
||
258 | // ids_to_link_to.push_back(0);
|
||
259 | // return ids_to_link_to; //only compare to first frame
|
||
260 | // } else if(max_targets == 1){
|
||
261 | // ids_to_link_to.push_back(graph_.size()-2);
|
||
262 | // return ids_to_link_to; //only compare to previous frame
|
||
263 | // }
|
||
264 | //
|
||
265 | // //Subset of the majority of the nodes
|
||
266 | // double id = 0.0; //always start with the first
|
||
267 | // double increment = max_id_plus1/(double)(max_targets);//max_targets-1 = intermediate steps
|
||
268 | // increment = increment < 1.0 ? 1.0 : increment; //smaller steps would select some id's twice
|
||
269 | // ROS_DEBUG("preparing loop %f %f", id, increment);
|
||
270 | // while((int)id < (int)max_id_plus1){
|
||
271 | // ids_to_link_to.push_back((int)id); //implicit rounding
|
||
272 | // id += increment;
|
||
273 | // }
|
||
274 | // //All the last few nodes
|
||
275 | // for(int i = 2; i <= (int)graph_.size() && i <= last_targets; i++){//start at two, b/c the prev node is always already checked in addNode{
|
||
276 | // ids_to_link_to.push_back(graph_.size()-i);
|
||
277 | // }
|
||
278 | // //output only loop
|
||
279 | // std::stringstream ss;
|
||
280 | // ss << "Node ID's to compare with candidate for node " << graph_.size() << ":";
|
||
281 | // for(int i = 0; i < (int)ids_to_link_to.size()-1; i++){
|
||
282 | // ss << ids_to_link_to[i] << ", " ;
|
||
283 | // }
|
||
284 | // ROS_DEBUG("%s%i", ss.str().c_str(), ids_to_link_to.last()); //print last one here, to avoid trailing ","
|
||
285 | // return ids_to_link_to;
|
||
286 | //}
|
||
287 | |||
288 | void GraphManager::resetGraph(){
|
||
289 | marker_id =0;
|
||
290 | delete optimizer_;
|
||
291 | // allocating the optimizer
|
||
292 | optimizer_ = new g2o::SparseOptimizer();
|
||
293 | //SlamLinearSolver* linearSolver = new SlamLinearSolver();
|
||
294 | SlamLinearCholmodSolver* linearSolver = new SlamLinearCholmodSolver();
|
||
295 | linearSolver->setBlockOrdering(false);
|
||
296 | SlamBlockSolver* solver = new SlamBlockSolver(optimizer_, linearSolver);
|
||
297 | optimizer_->setSolver(solver); |
||
298 | |||
299 | graph_.clear();//TODO: also delete the nodes
|
||
300 | Q_EMIT resetGLViewer(); |
||
301 | reset_request_ = false;
|
||
302 | } |
||
303 | |||
304 | // returns true, iff node could be added to the cloud
|
||
305 | bool GraphManager::addNode(Node* new_node) {
|
||
306 | struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime); |
||
307 | process_node_runs_ = true;
|
||
308 | |||
309 | last_inlier_matches_.clear(); |
||
310 | if(reset_request_) resetGraph();
|
||
311 | |||
312 | if (new_node->feature_locations_2d_.size() <= 50){ |
||
313 | ROS_DEBUG("found only %i features on image, node is not included",(int)new_node->feature_locations_2d_.size()); |
||
314 | process_node_runs_ = false;
|
||
315 | return false; |
||
316 | } |
||
317 | |||
318 | //set the node id only if the node is actually added to the graph
|
||
319 | //needs to be done here as the graph size can change inside this function
|
||
320 | new_node->id_ = graph_.size(); |
||
321 | ROS_DEBUG("New Node with id %i has address %p (GraphManager is %p)", new_node->id_, new_node, this); |
||
322 | |||
323 | //First Node, so only build its index, insert into storage and add a
|
||
324 | //vertex at the origin, of which the position is very certain
|
||
325 | if (graph_.size()==0){ |
||
326 | init_base_pose_ = new_node->getGroundTruthTransform();//identity if no MoCap available
|
||
327 | new_node->buildFlannIndex(); // create index so that next nodes can use it
|
||
328 | graph_[new_node->id_] = new_node; |
||
329 | g2o::VertexSE3* reference_pose = new g2o::VertexSE3;
|
||
330 | reference_pose->setId(0);
|
||
331 | reference_pose->setEstimate(g2o::SE3Quat()); |
||
332 | reference_pose->setFixed(true);//fix at origin |
||
333 | optimizer_mutex.lock(); |
||
334 | optimizer_->addVertex(reference_pose); |
||
335 | optimizer_mutex.unlock(); |
||
336 | QString message; |
||
337 | Q_EMIT setGUIInfo(message.sprintf("Added first node with %i keypoints to the graph", (int)new_node->feature_locations_2d_.size())); |
||
338 | pointcloud_type::Ptr the_pc = new_node->pc_col; |
||
339 | Q_EMIT setPointCloud(the_pc.get(), latest_transform_); |
||
340 | ROS_DEBUG("GraphManager is thread %d, New Node is at (%p, %p)", (unsigned int)QThread::currentThreadId(), new_node, graph_[0]); |
||
341 | process_node_runs_ = false;
|
||
342 | return true; |
||
343 | } |
||
344 | |||
345 | /*
|
||
346 | g2o::HyperDijkstra hypdij(optimizer_);
|
||
347 | g2o::UniformCostFunction f;
|
||
348 | g2o::VertexSE3* root_vertex = dynamic_cast<g2o::VertexSE3*>(optimizer_->vertex(0));
|
||
349 | hypdij.shortestPaths(root_vertex,&f);
|
||
350 | */
|
||
351 | |||
352 | |||
353 | unsigned int num_edges_before = optimizer_->edges().size(); |
||
354 | |||
355 | ROS_DEBUG("Graphsize: %d", (int) graph_.size()); |
||
356 | marker_id = 0; //overdraw old markers |
||
357 | last_matching_node_ = -1;
|
||
358 | |||
359 | |||
360 | //MAIN LOOP: Compare node pairs ######################################################################
|
||
361 | //First check if trafo to last frame is big
|
||
362 | Node* prev_frame = graph_[graph_.size()-1];
|
||
363 | ROS_INFO("Comparing new node (%i) with previous node %i / %i", new_node->id_, (int)graph_.size()-1, prev_frame->id_); |
||
364 | MatchingResult mr = new_node->matchNodePair(prev_frame); |
||
365 | if(mr.edge.id1 >= 0 && !isBigTrafo(mr.edge.mean)){ |
||
366 | ROS_WARN("Transformation not relevant. Did not add as Node");
|
||
367 | process_node_runs_ = false;
|
||
368 | return false; |
||
369 | } else if(mr.edge.id1 >= 0){ |
||
370 | |||
371 | //mr.edge.informationMatrix *= geodesicDiscount(hypdij, mr);
|
||
372 | ROS_INFO_STREAM("Information Matrix for Edge (" << mr.edge.id1 << "<->" << mr.edge.id2 << "\n" << mr.edge.informationMatrix); |
||
373 | |||
374 | if (addEdgeToG2O(mr.edge, true, true)) { |
||
375 | ROS_INFO("Added Edge between %i and %i. Inliers: %i",mr.edge.id1,mr.edge.id2,(int) mr.inlier_matches.size()); |
||
376 | last_matching_node_ = mr.edge.id1; |
||
377 | last_inlier_matches_ = mr.inlier_matches; |
||
378 | last_matches_ = mr.all_matches; |
||
379 | last_edge_ = mr.edge.mean; |
||
380 | |||
381 | } else {
|
||
382 | process_node_runs_ = false;
|
||
383 | return false; |
||
384 | } |
||
385 | } |
||
386 | //Eigen::Matrix4f ransac_trafo, final_trafo;
|
||
387 | QList<int> vertices_to_comp = getPotentialEdgeTargets(new_node, ParameterServer::instance()->get<int>("connectivity")); //vernetzungsgrad |
||
388 | QList<const Node* > nodes_to_comp;//only necessary for parallel computation |
||
389 | for (int id_of_id = (int)vertices_to_comp.size()-1; id_of_id >=0;id_of_id--){ |
||
390 | |||
391 | #ifndef CONCURRENT_EDGE_COMPUTATION
|
||
392 | #define QT_NO_CONCURRENT
|
||
393 | #endif
|
||
394 | #ifndef QT_NO_CONCURRENT
|
||
395 | //First compile a qlist of the nodes to be compared, then run the comparisons in parallel,
|
||
396 | //collecting a qlist of the results (using the blocking version of mapped).
|
||
397 | nodes_to_comp.push_back(graph_[vertices_to_comp[id_of_id]]); |
||
398 | } |
||
399 | QThreadPool* qtp = QThreadPool::globalInstance(); |
||
400 | ROS_INFO("Running node comparisons in parallel in %i (of %i) available threads", qtp->maxThreadCount() - qtp->activeThreadCount(), qtp->maxThreadCount());
|
||
401 | if( qtp->maxThreadCount() - qtp->activeThreadCount() == 1){ |
||
402 | ROS_WARN("Few Threads Remaining: Increasing maxThreadCount to %i", qtp->maxThreadCount()+1); |
||
403 | qtp->setMaxThreadCount(qtp->maxThreadCount()+1);
|
||
404 | } |
||
405 | QList<MatchingResult> results = QtConcurrent::blockingMapped(nodes_to_comp, boost::bind(&Node::matchNodePair, new_node, _1)); |
||
406 | for(int i = 0; i < results.size(); i++){ |
||
407 | MatchingResult& mr = results[i]; |
||
408 | #else
|
||
409 | Node* abcd = graph_[vertices_to_comp[id_of_id]]; |
||
410 | ROS_INFO("Comparing new node (%i) with node %i / %i", new_node->id_, vertices_to_comp[id_of_id], abcd->id_);
|
||
411 | MatchingResult mr = new_node->matchNodePair(abcd); |
||
412 | #endif
|
||
413 | if(mr.edge.id1 >= 0){ |
||
414 | //mr.edge.informationMatrix *= geodesicDiscount(hypdij, mr);
|
||
415 | ROS_INFO_STREAM("Information Matrix for Edge (" << mr.edge.id1 << "<->" << mr.edge.id2 << "\n" << mr.edge.informationMatrix); |
||
416 | |||
417 | if (addEdgeToG2O(mr.edge, isBigTrafo(mr.edge.mean), mr.inlier_matches.size() > last_inlier_matches_.size())) { //TODO: result isBigTrafo is not considered |
||
418 | ROS_INFO("Added Edge between %i and %i. Inliers: %i",mr.edge.id1,mr.edge.id2,(int) mr.inlier_matches.size()); |
||
419 | if(mr.inlier_matches.size() > last_inlier_matches_.size()){
|
||
420 | last_matching_node_ = mr.edge.id1; |
||
421 | last_inlier_matches_ = mr.inlier_matches; |
||
422 | last_matches_ = mr.all_matches; |
||
423 | } |
||
424 | } |
||
425 | } |
||
426 | } |
||
427 | //END OF MAIN LOOP: Compare node pairs ######################################################################
|
||
428 | |||
429 | if (optimizer_->edges().size() == num_edges_before) { //Failure: Create Bogus edge |
||
430 | ROS_WARN("Node %u could not be matched. Adding with constant motion assumption", (unsigned int)graph_.size()); |
||
431 | LoadedEdge3D virtual_edge; |
||
432 | virtual_edge.id1 = graph_.size()-1;
|
||
433 | virtual_edge.id2 = graph_.size(); |
||
434 | virtual_edge.mean = last_edge_; |
||
435 | virtual_edge.informationMatrix = Eigen::Matrix<double,6,6>::Identity();//*(1e-6); |
||
436 | addEdgeToG2O(virtual_edge, true,true) ; |
||
437 | } |
||
438 | |||
439 | |||
440 | |||
441 | if (optimizer_->edges().size() > num_edges_before) { //Success |
||
442 | new_node->buildFlannIndex(); |
||
443 | graph_[new_node->id_] = new_node; |
||
444 | ROS_INFO("Added Node, new Graphsize: %i", (int) graph_.size()); |
||
445 | if((optimizer_->vertices().size() % ParameterServer::instance()->get<int>("optimizer_skip_step")) == 0){ |
||
446 | optimizeGraph(); |
||
447 | } |
||
448 | //Q_EMIT updateTransforms(getAllPosesAsMatrixList());
|
||
449 | //Q_EMIT setGraphEdges(getGraphEdges());
|
||
450 | //make the transform of the last node known
|
||
451 | broadcastTransform(ros::TimerEvent()); |
||
452 | visualizeGraphEdges(); |
||
453 | visualizeGraphNodes(); |
||
454 | visualizeFeatureFlow3D(marker_id++); |
||
455 | if(last_matching_node_ <= 0){ cloudRendered(new_node->pc_col.get());}//delete points of non-matching nodes. They shall not be rendered |
||
456 | pointcloud_type::Ptr the_pc = new_node->pc_col; |
||
457 | Q_EMIT setPointCloud(the_pc.get(), latest_transform_); |
||
458 | ROS_DEBUG("GraphManager is thread %d", (unsigned int)QThread::currentThreadId()); |
||
459 | }else{
|
||
460 | ROS_WARN("Choosing new initial node, because it has more features");
|
||
461 | if(graph_.size() == 1){//if there is only one node which has less features, replace it by the new one |
||
462 | if(new_node->feature_locations_2d_.size() > graph_[0]->feature_locations_2d_.size()){ |
||
463 | this->resetGraph();
|
||
464 | process_node_runs_ = false;
|
||
465 | return this->addNode(new_node); |
||
466 | } |
||
467 | } else { //delete new_node; //is now done by auto_ptr |
||
468 | ROS_WARN("Did not add as Node");
|
||
469 | } |
||
470 | } |
||
471 | QString message; |
||
472 | clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s"); |
||
473 | Q_EMIT setGUIInfo(message.sprintf("%s, Graph Size: %iN/%iE, Duration: %f, Inliers: %i, χ<sup>2</sup>: %f",
|
||
474 | (optimizer_->edges().size() > num_edges_before) ? "Added" : "Ignored", |
||
475 | (int)optimizer_->vertices().size(), (int)optimizer_->edges().size(), |
||
476 | elapsed, (int)last_inlier_matches_.size(), optimizer_->chi2()));
|
||
477 | process_node_runs_ = false;
|
||
478 | return (optimizer_->edges().size() > num_edges_before);
|
||
479 | } |
||
480 | |||
481 | |||
482 | ///Get the norm of the translational part of an affine matrix (Helper for isBigTrafo)
|
||
483 | void GraphManager::visualizeFeatureFlow3D(unsigned int marker_id, |
||
484 | bool draw_outlier) const{ |
||
485 | struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime); |
||
486 | if (ransac_marker_pub_.getNumSubscribers() > 0){ //don't visualize, if nobody's looking |
||
487 | |||
488 | visualization_msgs::Marker marker_lines; |
||
489 | |||
490 | marker_lines.header.frame_id = "/openni_rgb_optical_frame";
|
||
491 | marker_lines.ns = "ransac_markers";
|
||
492 | marker_lines.header.stamp = ros::Time::now(); |
||
493 | marker_lines.action = visualization_msgs::Marker::ADD; |
||
494 | marker_lines.pose.orientation.w = 1.0; |
||
495 | marker_lines.id = marker_id; |
||
496 | marker_lines.type = visualization_msgs::Marker::LINE_LIST; |
||
497 | marker_lines.scale.x = 0.002; |
||
498 | |||
499 | std_msgs::ColorRGBA color_red ; //red outlier
|
||
500 | color_red.r = 1.0; |
||
501 | color_red.a = 1.0; |
||
502 | std_msgs::ColorRGBA color_green; //green inlier, newer endpoint
|
||
503 | color_green.g = 1.0; |
||
504 | color_green.a = 1.0; |
||
505 | std_msgs::ColorRGBA color_yellow; //yellow inlier, earlier endpoint
|
||
506 | color_yellow.r = 1.0; |
||
507 | color_yellow.g = 1.0; |
||
508 | color_yellow.a = 1.0; |
||
509 | std_msgs::ColorRGBA color_blue ; //red-blue outlier
|
||
510 | color_blue.b = 1.0; |
||
511 | color_blue.a = 1.0; |
||
512 | |||
513 | marker_lines.color = color_green; //just to set the alpha channel to non-zero
|
||
514 | const g2o::VertexSE3* earlier_v; //used to get the transform |
||
515 | const g2o::VertexSE3* newer_v; //used to get the transform |
||
516 | VertexIDMap v_idmap = optimizer_->vertices(); |
||
517 | // end of initialization
|
||
518 | ROS_DEBUG("Matches Visualization start: %lu Matches, %lu Inliers", last_matches_.size(), last_inlier_matches_.size());
|
||
519 | |||
520 | // write all inital matches to the line_list
|
||
521 | marker_lines.points.clear();//necessary?
|
||
522 | |||
523 | if (draw_outlier)
|
||
524 | { |
||
525 | for (unsigned int i=0;i<last_matches_.size(); i++){ |
||
526 | int newer_id = last_matches_.at(i).queryIdx; //feature id in newer node |
||
527 | int earlier_id = last_matches_.at(i).trainIdx; //feature id in earlier node |
||
528 | |||
529 | earlier_v = dynamic_cast<g2o::VertexSE3*>(optimizer_->vertex(last_matching_node_));
|
||
530 | newer_v = dynamic_cast<g2o::VertexSE3*>(optimizer_->vertex(graph_.size()-1)); |
||
531 | |||
532 | //Outliers are red (newer) to blue (older)
|
||
533 | marker_lines.colors.push_back(color_red); |
||
534 | marker_lines.colors.push_back(color_blue); |
||
535 | |||
536 | Node* last = graph_.find(graph_.size()-1)->second;
|
||
537 | marker_lines.points.push_back( |
||
538 | pointInWorldFrame(last->feature_locations_3d_[newer_id], newer_v->estimate())); |
||
539 | Node* prev = graph_.find(last_matching_node_)->second; |
||
540 | marker_lines.points.push_back( |
||
541 | pointInWorldFrame(prev->feature_locations_3d_[earlier_id], earlier_v->estimate())); |
||
542 | } |
||
543 | } |
||
544 | |||
545 | for (unsigned int i=0;i<last_inlier_matches_.size(); i++){ |
||
546 | int newer_id = last_inlier_matches_.at(i).queryIdx; //feature id in newer node |
||
547 | int earlier_id = last_inlier_matches_.at(i).trainIdx; //feature id in earlier node |
||
548 | |||
549 | earlier_v = dynamic_cast<g2o::VertexSE3*>(optimizer_->vertex(last_matching_node_));
|
||
550 | newer_v = dynamic_cast<g2o::VertexSE3*>(optimizer_->vertex(graph_.size()-1)); |
||
551 | |||
552 | |||
553 | //inliers are green (newer) to blue (older)
|
||
554 | marker_lines.colors.push_back(color_green); |
||
555 | marker_lines.colors.push_back(color_blue); |
||
556 | |||
557 | Node* last = graph_.find(graph_.size()-1)->second;
|
||
558 | marker_lines.points.push_back( |
||
559 | pointInWorldFrame(last->feature_locations_3d_[newer_id], newer_v->estimate())); |
||
560 | Node* prev = graph_.find(last_matching_node_)->second; |
||
561 | marker_lines.points.push_back( |
||
562 | pointInWorldFrame(prev->feature_locations_3d_[earlier_id], earlier_v->estimate())); |
||
563 | } |
||
564 | |||
565 | ransac_marker_pub_.publish(marker_lines); |
||
566 | ROS_DEBUG_STREAM("Published " << marker_lines.points.size()/2 << " lines"); |
||
567 | } |
||
568 | clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s"); |
||
569 | } |
||
570 | |||
571 | |||
572 | QList<QPair<int, int> >* GraphManager::getGraphEdges() const { |
||
573 | struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime); |
||
574 | QList<QPair<int, int> >* edge_list = new QList<QPair<int, int> >(); |
||
575 | g2o::VertexSE3 *v1, *v2; //used in loop
|
||
576 | EdgeSet::iterator edge_iter = optimizer_->edges().begin(); |
||
577 | for(;edge_iter != optimizer_->edges().end(); edge_iter++) {
|
||
578 | g2o::EdgeSE3* myedge = dynamic_cast<g2o::EdgeSE3*>(*edge_iter);
|
||
579 | std::vector<g2o::HyperGraph::Vertex*>& myvertices = myedge->vertices(); |
||
580 | v1 = dynamic_cast<g2o::VertexSE3*>(myvertices.at(1)); |
||
581 | v2 = dynamic_cast<g2o::VertexSE3*>(myvertices.at(0)); |
||
582 | edge_list->append( qMakePair(v1->id(), v2->id())); |
||
583 | } |
||
584 | clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s"); |
||
585 | return edge_list;
|
||
586 | } |
||
587 | void GraphManager::visualizeGraphEdges() const { |
||
588 | struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime); |
||
589 | |||
590 | if (marker_pub_.getNumSubscribers() > 0){ //no visualization for nobody |
||
591 | visualization_msgs::Marker edges_marker; |
||
592 | edges_marker.header.frame_id = "/openni_rgb_optical_frame"; //TODO: Should be a meaningfull fixed frame with known relative pose to the camera |
||
593 | edges_marker.header.stamp = ros::Time::now(); |
||
594 | edges_marker.ns = "camera_pose_graph"; // Set the namespace and id for this marker. This serves to create a unique ID |
||
595 | edges_marker.id = 0; // Any marker sent with the same namespace and id will overwrite the old one |
||
596 | |||
597 | edges_marker.type = visualization_msgs::Marker::LINE_LIST; |
||
598 | edges_marker.action = visualization_msgs::Marker::ADD; // Set the marker action. Options are ADD and DELETE
|
||
599 | edges_marker.frame_locked = true; //rviz automatically retransforms the markers into the frame every update cycle |
||
600 | // Set the scale of the marker -- 1x1x1 here means 1m on a side
|
||
601 | edges_marker.scale.x = 0.005; //line width |
||
602 | //Global pose (used to transform all points)
|
||
603 | edges_marker.pose.position.x = 0;
|
||
604 | edges_marker.pose.position.y = 0;
|
||
605 | edges_marker.pose.position.z = 0;
|
||
606 | edges_marker.pose.orientation.x = 0.0; |
||
607 | edges_marker.pose.orientation.y = 0.0; |
||
608 | edges_marker.pose.orientation.z = 0.0; |
||
609 | edges_marker.pose.orientation.w = 1.0; |
||
610 | // Set the color -- be sure to set alpha to something non-zero!
|
||
611 | edges_marker.color.r = 1.0f; |
||
612 | edges_marker.color.g = 1.0f; |
||
613 | edges_marker.color.b = 1.0f; |
||
614 | edges_marker.color.a = 0.5f;//looks smoother |
||
615 | geometry_msgs::Point point; //start and endpoint for each line segment
|
||
616 | g2o::VertexSE3* v1,* v2; //used in loop
|
||
617 | EdgeSet::iterator edge_iter = optimizer_->edges().begin(); |
||
618 | int counter = 0; |
||
619 | for(;edge_iter != optimizer_->edges().end(); edge_iter++, counter++) {
|
||
620 | g2o::EdgeSE3* myedge = dynamic_cast<g2o::EdgeSE3*>(*edge_iter);
|
||
621 | std::vector<g2o::HyperGraph::Vertex*>& myvertices = myedge->vertices(); |
||
622 | v1 = dynamic_cast<g2o::VertexSE3*>(myvertices.at(1)); |
||
623 | v2 = dynamic_cast<g2o::VertexSE3*>(myvertices.at(0)); |
||
624 | |||
625 | point.x = v1->estimate().translation().x(); |
||
626 | point.y = v1->estimate().translation().y(); |
||
627 | point.z = v1->estimate().translation().z(); |
||
628 | edges_marker.points.push_back(point); |
||
629 | |||
630 | point.x = v2->estimate().translation().x(); |
||
631 | point.y = v2->estimate().translation().y(); |
||
632 | point.z = v2->estimate().translation().z(); |
||
633 | edges_marker.points.push_back(point); |
||
634 | } |
||
635 | |||
636 | marker_pub_.publish (edges_marker); |
||
637 | ROS_INFO("published %d graph edges", counter);
|
||
638 | } |
||
639 | |||
640 | clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s"); |
||
641 | } |
||
642 | |||
643 | void GraphManager::visualizeGraphNodes() const { |
||
644 | struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime); |
||
645 | |||
646 | if (marker_pub_.getNumSubscribers() > 0){ //don't visualize, if nobody's looking |
||
647 | visualization_msgs::Marker nodes_marker; |
||
648 | nodes_marker.header.frame_id = "/openni_rgb_optical_frame"; //TODO: Should be a meaningfull fixed frame with known relative pose to the camera |
||
649 | nodes_marker.header.stamp = ros::Time::now(); |
||
650 | nodes_marker.ns = "camera_pose_graph"; // Set the namespace and id for this marker. This serves to create a unique ID |
||
651 | nodes_marker.id = 1; // Any marker sent with the same namespace and id will overwrite the old one |
||
652 | |||
653 | |||
654 | nodes_marker.type = visualization_msgs::Marker::LINE_LIST; |
||
655 | nodes_marker.action = visualization_msgs::Marker::ADD; // Set the marker action. Options are ADD and DELETE
|
||
656 | nodes_marker.frame_locked = true; //rviz automatically retransforms the markers into the frame every update cycle |
||
657 | // Set the scale of the marker -- 1x1x1 here means 1m on a side
|
||
658 | nodes_marker.scale.x = 0.002; |
||
659 | //Global pose (used to transform all points) //TODO: is this the default pose anyway?
|
||
660 | nodes_marker.pose.position.x = 0;
|
||
661 | nodes_marker.pose.position.y = 0;
|
||
662 | nodes_marker.pose.position.z = 0;
|
||
663 | nodes_marker.pose.orientation.x = 0.0; |
||
664 | nodes_marker.pose.orientation.y = 0.0; |
||
665 | nodes_marker.pose.orientation.z = 0.0; |
||
666 | nodes_marker.pose.orientation.w = 1.0; |
||
667 | // Set the color -- be sure to set alpha to something non-zero!
|
||
668 | nodes_marker.color.r = 1.0f; |
||
669 | nodes_marker.color.g = 0.0f; |
||
670 | nodes_marker.color.b = 0.0f; |
||
671 | nodes_marker.color.a = 1.0f; |
||
672 | |||
673 | |||
674 | geometry_msgs::Point tail; //same startpoint for each line segment
|
||
675 | geometry_msgs::Point tip; //different endpoint for each line segment
|
||
676 | std_msgs::ColorRGBA arrow_color_red ; //red x axis
|
||
677 | arrow_color_red.r = 1.0; |
||
678 | arrow_color_red.a = 1.0; |
||
679 | std_msgs::ColorRGBA arrow_color_green; //green y axis
|
||
680 | arrow_color_green.g = 1.0; |
||
681 | arrow_color_green.a = 1.0; |
||
682 | std_msgs::ColorRGBA arrow_color_blue ; //blue z axis
|
||
683 | arrow_color_blue.b = 1.0; |
||
684 | arrow_color_blue.a = 1.0; |
||
685 | Eigen::Vector3d origin(0.0,0.0,0.0); |
||
686 | Eigen::Vector3d x_axis(0.2,0.0,0.0); //20cm long axis for the first (almost fixed) node |
||
687 | Eigen::Vector3d y_axis(0.0,0.2,0.0); |
||
688 | Eigen::Vector3d z_axis(0.0,0.0,0.2); |
||
689 | Eigen::Vector3d tmp; //the transformed endpoints
|
||
690 | int counter = 0; |
||
691 | g2o::VertexSE3* v; //used in loop
|
||
692 | VertexIDMap::iterator vertex_iter = optimizer_->vertices().begin(); |
||
693 | for(/*see above*/; vertex_iter != optimizer_->vertices().end(); vertex_iter++, counter++) { |
||
694 | v = dynamic_cast<g2o::VertexSE3* >((*vertex_iter).second);
|
||
695 | //v->estimate().rotation().x()+ v->estimate().rotation().y()+ v->estimate().rotation().z()+ v->estimate().rotation().w();
|
||
696 | tmp = v->estimate() * origin; |
||
697 | tail.x = tmp.x(); |
||
698 | tail.y = tmp.y(); |
||
699 | tail.z = tmp.z(); |
||
700 | //Endpoints X-Axis
|
||
701 | nodes_marker.points.push_back(tail); |
||
702 | nodes_marker.colors.push_back(arrow_color_red); |
||
703 | tmp = v->estimate() * x_axis; |
||
704 | tip.x = tmp.x(); |
||
705 | tip.y = tmp.y(); |
||
706 | tip.z = tmp.z(); |
||
707 | nodes_marker.points.push_back(tip); |
||
708 | nodes_marker.colors.push_back(arrow_color_red); |
||
709 | //Endpoints Y-Axis
|
||
710 | nodes_marker.points.push_back(tail); |
||
711 | nodes_marker.colors.push_back(arrow_color_green); |
||
712 | tmp = v->estimate() * y_axis; |
||
713 | tip.x = tmp.x(); |
||
714 | tip.y = tmp.y(); |
||
715 | tip.z = tmp.z(); |
||
716 | nodes_marker.points.push_back(tip); |
||
717 | nodes_marker.colors.push_back(arrow_color_green); |
||
718 | //Endpoints Z-Axis
|
||
719 | nodes_marker.points.push_back(tail); |
||
720 | nodes_marker.colors.push_back(arrow_color_blue); |
||
721 | tmp = v->estimate() * z_axis; |
||
722 | tip.x = tmp.x(); |
||
723 | tip.y = tmp.y(); |
||
724 | tip.z = tmp.z(); |
||
725 | nodes_marker.points.push_back(tip); |
||
726 | nodes_marker.colors.push_back(arrow_color_blue); |
||
727 | //shorten all nodes after the first one
|
||
728 | x_axis.x() = 0.1; |
||
729 | y_axis.y() = 0.1; |
||
730 | z_axis.z() = 0.1; |
||
731 | } |
||
732 | |||
733 | marker_pub_.publish (nodes_marker); |
||
734 | ROS_INFO("published %d graph nodes", counter);
|
||
735 | } |
||
736 | |||
737 | clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s"); |
||
738 | } |
||
739 | |||
740 | bool GraphManager::addEdgeToG2O(const LoadedEdge3D& edge, bool largeEdge, bool set_estimate) { |
||
741 | struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime); |
||
742 | |||
743 | QMutexLocker locker(&optimizer_mutex); |
||
744 | g2o::VertexSE3* v1 = dynamic_cast<g2o::VertexSE3*>(optimizer_->vertex(edge.id1));
|
||
745 | g2o::VertexSE3* v2 = dynamic_cast<g2o::VertexSE3*>(optimizer_->vertex(edge.id2));
|
||
746 | |||
747 | // at least one vertex has to be created, assert that the transformation
|
||
748 | // is large enough to avoid to many vertices on the same spot
|
||
749 | if (!v1 || !v2){
|
||
750 | if (!largeEdge) {
|
||
751 | ROS_INFO("Edge to new vertex is to short, vertex will not be inserted");
|
||
752 | return false; |
||
753 | } |
||
754 | } |
||
755 | |||
756 | if(!v1 && !v2){
|
||
757 | ROS_ERROR("Missing both vertices: %i, %i, cannot create edge", edge.id1, edge.id2);
|
||
758 | return false; |
||
759 | } |
||
760 | else if (!v1 && v2) { |
||
761 | v1 = new g2o::VertexSE3;
|
||
762 | assert(v1); |
||
763 | v1->setId(edge.id1); |
||
764 | v1->setEstimate(v2->estimate() * edge.mean.inverse()); |
||
765 | optimizer_->addVertex(v1); |
||
766 | latest_transform_ = g2o2QMatrix(v1->estimate()); |
||
767 | } |
||
768 | else if (!v2 && v1) { |
||
769 | v2 = new g2o::VertexSE3;
|
||
770 | assert(v2); |
||
771 | v2->setId(edge.id2); |
||
772 | v2->setEstimate(v1->estimate() * edge.mean); |
||
773 | optimizer_->addVertex(v2); |
||
774 | latest_transform_ = g2o2QMatrix(v2->estimate()); |
||
775 | } |
||
776 | else if(set_estimate){ |
||
777 | v2->setEstimate(v1->estimate() * edge.mean); |
||
778 | } |
||
779 | g2o::EdgeSE3* g2o_edge = new g2o::EdgeSE3;
|
||
780 | g2o_edge->vertices()[0] = v1;
|
||
781 | g2o_edge->vertices()[1] = v2;
|
||
782 | g2o_edge->setMeasurement(edge.mean); |
||
783 | g2o_edge->setInverseMeasurement(edge.mean.inverse()); |
||
784 | g2o_edge->setInformation(edge.informationMatrix); |
||
785 | optimizer_->addEdge(g2o_edge); |
||
786 | |||
787 | clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s"); |
||
788 | return true; |
||
789 | } |
||
790 | |||
791 | void GraphManager::optimizeGraph(int iter){ |
||
792 | struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime); |
||
793 | int iterations = iter >= 0 ? iter : ParameterServer::instance()->get<int>("optimizer_iterations"); |
||
794 | QMutexLocker locker(&optimizer_mutex); |
||
795 | |||
796 | ROS_WARN("Starting Optimization");
|
||
797 | std::string bagfile_name = ParameterServer::instance()->get<std::string>("bagfile_name"); |
||
798 | optimizer_->save((bagfile_name + "_g2o-optimizer-save-file-before").c_str());
|
||
799 | optimizer_->initializeOptimization(); |
||
800 | for(int i = 0; i < iterations; i++){ |
||
801 | int currentIt = optimizer_->optimize(1); |
||
802 | optimizer_->computeActiveErrors(); |
||
803 | ROS_INFO_STREAM("G2O Statistics: " << optimizer_->vertices().size() << " nodes, " |
||
804 | << optimizer_->edges().size() << " edges. "
|
||
805 | << "chi2: " << optimizer_->chi2() << ", Iterations: " << currentIt); |
||
806 | } |
||
807 | optimizer_->save((bagfile_name + "_g2o-optimizer-save-file-after").c_str());
|
||
808 | |||
809 | |||
810 | g2o::VertexSE3* v = dynamic_cast<g2o::VertexSE3*>(optimizer_->vertex(optimizer_->vertices().size()-1)); |
||
811 | |||
812 | computed_motion_ = g2o2TF(v->estimate()); |
||
813 | latest_transform_ = g2o2QMatrix(v->estimate()); |
||
814 | Q_EMIT updateTransforms(getAllPosesAsMatrixList()); |
||
815 | Q_EMIT setGraphEdges(getGraphEdges()); |
||
816 | |||
817 | clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s"); |
||
818 | } |
||
819 | |||
820 | void GraphManager::broadcastTransform(const ros::TimerEvent& ){ |
||
821 | ros::Time now = ros::Time::now(); |
||
822 | std::string fixed_frame = ParameterServer::instance()->get<std::string>("fixed_frame_name"); |
||
823 | std::string base_frame = ParameterServer::instance()->get<std::string>("base_frame_name"); |
||
824 | |||
825 | if(graph_.size() > 0 && !batch_processing_runs_ ) { |
||
826 | Node* node = graph_[graph_.size()-1]; //latest node |
||
827 | base2points_ = node->getBase2PointsTransform(); |
||
828 | |||
829 | tf::Transform world2base; |
||
830 | world2base = init_base_pose_*base2points_*computed_motion_*base2points_.inverse(); |
||
831 | //br_.sendTransform(tf::StampedTransform(world2base, now, fixed_frame, base_frame));
|
||
832 | |||
833 | //visualize the transformation
|
||
834 | std::stringstream ss; |
||
835 | Eigen::Matrix4f transform; |
||
836 | pcl_ros::transformAsMatrix(computed_motion_, transform); |
||
837 | ss << "<b>Current Camera Transformation w.r.t. the Initial Frame</b>";
|
||
838 | ss << "<pre>" << transform << "</pre>"; |
||
839 | QString mystring(ss.str().c_str()); |
||
840 | Q_EMIT newTransformationMatrix(mystring); |
||
841 | } |
||
842 | else //no graph, no map, send initial pose (usually identity) |
||
843 | { |
||
844 | //br_.sendTransform(tf::StampedTransform(init_base_pose_, now, fixed_frame, base_frame));
|
||
845 | } |
||
846 | |||
847 | } |
||
848 | |||
849 | /**
|
||
850 | * Publish the updated transforms for the graph node resp. clouds
|
||
851 | *
|
||
852 | void GraphManager::publishCorrectedTransforms(){
|
||
853 | struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime);
|
||
854 | //fill message
|
||
855 | rgbdslam::CloudTransforms msg;
|
||
856 | for (unsigned int i = 0; i < optimizer_->vertices().size(); ++i) {
|
||
857 | g2o::VertexSE3* v = optimizer_->vertex(i);
|
||
858 | tf::Transform trans = g2o2TF(v->estimate());
|
||
859 | geometry_msgs::Transform trans_msg;
|
||
860 | tf::transformTFToMsg(trans,trans_msg);
|
||
861 | msg.transforms.push_back(trans_msg);
|
||
862 | msg.ids.push_back(graph_[i]->msg_id_); //msg_id is no more
|
||
863 | }
|
||
864 | msg.header.stamp = ros::Time::now();
|
||
865 | |||
866 | if (transform_pub_.getNumSubscribers() > 0)
|
||
867 | transform_pub_.publish(msg);
|
||
868 | clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s");
|
||
869 | }*/
|
||
870 | |||
871 | void GraphManager::reset(){
|
||
872 | reset_request_ = true;
|
||
873 | } |
||
874 | |||
875 | void GraphManager::deleteLastFrame(){
|
||
876 | if(graph_.size() <= 1) { |
||
877 | ROS_INFO("Resetting, as the only node is to be deleted");
|
||
878 | reset_request_ = true;
|
||
879 | Q_EMIT deleteLastNode(); |
||
880 | return;
|
||
881 | } |
||
882 | optimizer_mutex.lock(); |
||
883 | g2o::VertexSE3* v_to_del = dynamic_cast<g2o::VertexSE3*>(optimizer_->vertex(optimizer_->vertices().size()-1));//last vertex |
||
884 | g2o::VertexSE3 *v1, *v2; //used in loop as temporaries
|
||
885 | EdgeSet::iterator edge_iter = optimizer_->edges().begin(); |
||
886 | for(;edge_iter != optimizer_->edges().end(); edge_iter++) {
|
||
887 | g2o::EdgeSE3* myedge = dynamic_cast<g2o::EdgeSE3*>(*edge_iter);
|
||
888 | std::vector<g2o::HyperGraph::Vertex*>& myvertices = myedge->vertices(); |
||
889 | v1 = dynamic_cast<g2o::VertexSE3*>(myvertices.at(1)); |
||
890 | v2 = dynamic_cast<g2o::VertexSE3*>(myvertices.at(0)); |
||
891 | if(v1->id() == v_to_del->id() || v2->id() == v_to_del->id())
|
||
892 | optimizer_->removeEdge((*edge_iter)); |
||
893 | } |
||
894 | |||
895 | optimizer_->removeVertex(v_to_del); |
||
896 | graph_.erase(graph_.size()-1);
|
||
897 | optimizer_mutex.unlock(); |
||
898 | Q_EMIT deleteLastNode(); |
||
899 | optimizeGraph();//s.t. the effect of the removed edge transforms are removed to
|
||
900 | ROS_INFO("Removed most recent node");
|
||
901 | Q_EMIT setGUIInfo("Removed most recent node");
|
||
902 | //Q_EMIT setGraphEdges(getGraphEdges());
|
||
903 | //updateTransforms needs to be last, as it triggers a redraw
|
||
904 | //Q_EMIT updateTransforms(getAllPosesAsMatrixList());
|
||
905 | } |
||
906 | |||
907 | QList<QMatrix4x4>* GraphManager::getAllPosesAsMatrixList(){ |
||
908 | struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime); |
||
909 | ROS_DEBUG("Retrieving all transformations from optimizer");
|
||
910 | QList<QMatrix4x4>* result = new QList<QMatrix4x4>();
|
||
911 | #if defined(QT_VERSION) && QT_VERSION >= 0x040700 |
||
912 | result->reserve(optimizer_->vertices().size());//only allocates the internal pointer array
|
||
913 | #endif
|
||
914 | |||
915 | for (unsigned int i = 0; i < optimizer_->vertices().size(); ++i) { |
||
916 | g2o::VertexSE3* v = dynamic_cast<g2o::VertexSE3*>(optimizer_->vertex(i));
|
||
917 | if(v){
|
||
918 | result->push_back(g2o2QMatrix(v->estimate())); |
||
919 | } else {
|
||
920 | ROS_ERROR("Nullpointer in graph at position %i!", i);
|
||
921 | } |
||
922 | } |
||
923 | clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s"); |
||
924 | return result;
|
||
925 | } |
||
926 | |||
927 | // If QT Concurrent is available, run the saving in a seperate thread
|
||
928 | void GraphManager::saveAllClouds(QString filename, bool threaded){ |
||
929 | #ifndef QT_NO_CONCURRENT
|
||
930 | if(threaded)
|
||
931 | QFuture<void> f1 = QtConcurrent::run(this, &GraphManager::saveAllCloudsToFile, filename); |
||
932 | //f1.waitForFinished();
|
||
933 | else // Otherwise just call it without threading |
||
934 | #endif
|
||
935 | saveAllCloudsToFile(filename); |
||
936 | } |
||
937 | |||
938 | void GraphManager::saveIndividualClouds(QString filename, bool threaded){ |
||
939 | #ifndef QT_NO_CONCURRENT
|
||
940 | if(threaded)
|
||
941 | QFuture<void> f1 = QtConcurrent::run(this, &GraphManager::saveIndividualCloudsToFile, filename); |
||
942 | //f1.waitForFinished();
|
||
943 | else
|
||
944 | #endif
|
||
945 | saveIndividualCloudsToFile(filename); |
||
946 | } |
||
947 | |||
948 | void GraphManager::saveIndividualCloudsToFile(QString file_basename)
|
||
949 | { |
||
950 | struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime); |
||
951 | ROS_INFO("Saving all clouds to %sxxxx.pcd", qPrintable(file_basename));
|
||
952 | std::string gt = ParameterServer::instance()->get<std::string>("ground_truth_frame_name"); |
||
953 | ROS_INFO_COND(!gt.empty(), "Saving all clouds with ground truth sensor position to gt_%sxxxx.pcd", qPrintable(file_basename));
|
||
954 | |||
955 | batch_processing_runs_ = true;
|
||
956 | tf::Transform world2base; |
||
957 | QString message, filename; |
||
958 | std::string fixed_frame_id = ParameterServer::instance()->get<std::string>("fixed_frame_name"); |
||
959 | for (unsigned int i = 0; i < optimizer_->vertices().size(); ++i) { |
||
960 | g2o::VertexSE3* v = dynamic_cast<g2o::VertexSE3*>(optimizer_->vertex(i));
|
||
961 | if(!v){
|
||
962 | ROS_ERROR("Nullpointer in graph at position %i!", i);
|
||
963 | continue;
|
||
964 | } |
||
965 | if(graph_[i]->pc_col->size() == 0){ |
||
966 | ROS_INFO("Skipping Node %i, point cloud data is empty!", i);
|
||
967 | continue;
|
||
968 | } |
||
969 | /*/TODO: is all this correct?
|
||
970 | tf::Transform transform = g2o2TF(v->estimate());
|
||
971 | tf::Transform cam2rgb;
|
||
972 | cam2rgb.setRotation(tf::createQuaternionFromRPY(-1.57,0,-1.57));
|
||
973 | cam2rgb.setOrigin(tf::Point(0,-0.04,0));
|
||
974 | world2base = cam2rgb*transform;
|
||
975 | */
|
||
976 | tf::Transform pose = g2o2TF(v->estimate()); |
||
977 | tf::StampedTransform base2points = graph_[i]->getBase2PointsTransform();//get pose of base w.r.t current pc at capture time
|
||
978 | world2base = init_base_pose_*base2points*pose*base2points.inverse(); |
||
979 | |||
980 | Eigen::Vector4f sensor_origin(world2base.getOrigin().x(),world2base.getOrigin().y(),world2base.getOrigin().z(),world2base.getOrigin().w()); |
||
981 | Eigen::Quaternionf sensor_orientation(world2base.getRotation().w(),world2base.getRotation().x(),world2base.getRotation().y(),world2base.getRotation().z()); |
||
982 | |||
983 | graph_[i]->pc_col->sensor_origin_ = sensor_origin; |
||
984 | graph_[i]->pc_col->sensor_orientation_ = sensor_orientation; |
||
985 | graph_[i]->pc_col->header.frame_id = fixed_frame_id; |
||
986 | |||
987 | filename.sprintf("%s_%04d.pcd", qPrintable(file_basename), i);
|
||
988 | Q_EMIT setGUIStatus(message.sprintf("Saving to %s: Transformed Node %i/%i", qPrintable(filename), i, (int)optimizer_->vertices().size())); |
||
989 | pcl::io::savePCDFile(qPrintable(filename), *(graph_[i]->pc_col), true); //Last arg: true is binary mode. ASCII mode drops color bits |
||
990 | |||
991 | if(!gt.empty()){
|
||
992 | tf::StampedTransform gt_world2base = graph_[i ]->getGroundTruthTransform();//get mocap pose of base in map
|
||
993 | if( gt_world2base.frame_id_ == "/missing_ground_truth" ){ |
||
994 | ROS_WARN_STREAM("Skipping ground truth: " << gt_world2base.child_frame_id_ << " child/parent " << gt_world2base.frame_id_); |
||
995 | continue;
|
||
996 | } |
||
997 | Eigen::Vector4f sensor_origin(gt_world2base.getOrigin().x(),gt_world2base.getOrigin().y(),gt_world2base.getOrigin().z(),gt_world2base.getOrigin().w()); |
||
998 | Eigen::Quaternionf sensor_orientation(gt_world2base.getRotation().w(),gt_world2base.getRotation().x(),gt_world2base.getRotation().y(),gt_world2base.getRotation().z()); |
||
999 | |||
1000 | graph_[i]->pc_col->sensor_origin_ = sensor_origin; |
||
1001 | graph_[i]->pc_col->sensor_orientation_ = sensor_orientation; |
||
1002 | graph_[i]->pc_col->header.frame_id = fixed_frame_id; |
||
1003 | |||
1004 | filename.sprintf("%s_%04d_gt.pcd", qPrintable(file_basename), i);
|
||
1005 | Q_EMIT setGUIStatus(message.sprintf("Saving to %s: Transformed Node %i/%i", qPrintable(filename), i, (int)optimizer_->vertices().size())); |
||
1006 | pcl::io::savePCDFile(qPrintable(filename), *(graph_[i]->pc_col), true); //Last arg: true is binary mode. ASCII mode drops color bits |
||
1007 | } |
||
1008 | |||
1009 | } |
||
1010 | Q_EMIT setGUIStatus("Saved all point clouds");
|
||
1011 | ROS_INFO ("Saved all points clouds to %sxxxx.pcd", qPrintable(file_basename));
|
||
1012 | batch_processing_runs_ = false;
|
||
1013 | clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s"); |
||
1014 | } |
||
1015 | |||
1016 | void GraphManager::saveAllCloudsToFile(QString filename){
|
||
1017 | struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime); |
||
1018 | pointcloud_type aggregate_cloud; ///will hold all other clouds
|
||
1019 | ROS_INFO("Saving all clouds to %s, this may take a while as they need to be transformed to a common coordinate frame.", qPrintable(filename));
|
||
1020 | batch_processing_runs_ = true;
|
||
1021 | tf::Transform world2cam; |
||
1022 | //fill message
|
||
1023 | //rgbdslam::CloudTransforms msg;
|
||
1024 | QString message; |
||
1025 | tf::Transform cam2rgb; |
||
1026 | cam2rgb.setRotation(tf::createQuaternionFromRPY(-1.57,0,-1.57)); |
||
1027 | cam2rgb.setOrigin(tf::Point(0,-0.04,0)); |
||
1028 | for (unsigned int i = 0; i < optimizer_->vertices().size(); ++i) { |
||
1029 | g2o::VertexSE3* v = dynamic_cast<g2o::VertexSE3*>(optimizer_->vertex(i));
|
||
1030 | if(!v){
|
||
1031 | ROS_ERROR("Nullpointer in graph at position %i!", i);
|
||
1032 | continue;
|
||
1033 | } |
||
1034 | tf::Transform transform = g2o2TF(v->estimate()); |
||
1035 | world2cam = cam2rgb*transform; |
||
1036 | transformAndAppendPointCloud (*(graph_[i]->pc_col), aggregate_cloud, world2cam, Max_Depth); |
||
1037 | |||
1038 | if(ParameterServer::instance()->get<bool>("batch_processing")) |
||
1039 | graph_[i]->clearPointCloud(); //saving all is the last thing to do, so these are not required anymore
|
||
1040 | Q_EMIT setGUIStatus(message.sprintf("Saving to %s: Transformed Node %i/%i", qPrintable(filename), i, (int)optimizer_->vertices().size())); |
||
1041 | } |
||
1042 | aggregate_cloud.header.frame_id = "/openni_camera";
|
||
1043 | if(filename.endsWith(".ply", Qt::CaseInsensitive)) |
||
1044 | pointCloud2MeshFile(filename, aggregate_cloud); |
||
1045 | if(filename.endsWith(".pcd", Qt::CaseInsensitive)) |
||
1046 | pcl::io::savePCDFile(qPrintable(filename), aggregate_cloud, true); //Last arg is binary mode |
||
1047 | else {
|
||
1048 | ROS_WARN("Filename misses correct extension (.pcd or .ply) using .pcd");
|
||
1049 | filename.append(".pcd");
|
||
1050 | pcl::io::savePCDFile(qPrintable(filename), aggregate_cloud, true); //Last arg is binary mode |
||
1051 | } |
||
1052 | Q_EMIT setGUIStatus(message.sprintf("Saved %d data points to %s", (int)aggregate_cloud.points.size(), qPrintable(filename))); |
||
1053 | ROS_INFO ("Saved %d data points to %s", (int)aggregate_cloud.points.size(), qPrintable(filename)); |
||
1054 | |||
1055 | if (whole_cloud_pub_.getNumSubscribers() > 0){ //if it should also be send out |
||
1056 | sensor_msgs::PointCloud2 cloudMessage_; //this will be send out in batch mode
|
||
1057 | pcl::toROSMsg(aggregate_cloud,cloudMessage_); |
||
1058 | cloudMessage_.header.frame_id = "/openni_camera";
|
||
1059 | cloudMessage_.header.stamp = ros::Time::now(); |
||
1060 | whole_cloud_pub_.publish(cloudMessage_); |
||
1061 | ROS_INFO("Aggregate pointcloud sent");
|
||
1062 | } |
||
1063 | batch_processing_runs_ = false;
|
||
1064 | clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s"); |
||
1065 | } |
||
1066 | |||
1067 | void GraphManager::pointCloud2MeshFile(QString filename, pointcloud_type full_cloud){
|
||
1068 | QFile file(filename);//file is closed on destruction
|
||
1069 | if(!file.open(QIODevice::WriteOnly|QIODevice::Text)) return; //TODO: Errormessage |
||
1070 | QTextStream out(&file); |
||
1071 | out << "ply\n";
|
||
1072 | out << "format ascii 1.0\n";
|
||
1073 | out << "element vertex " << (int)full_cloud.points.size() << "\n"; |
||
1074 | out << "property float x\n";
|
||
1075 | out << "property float y\n";
|
||
1076 | out << "property float z\n";
|
||
1077 | out << "property uchar red\n";
|
||
1078 | out << "property uchar green\n";
|
||
1079 | out << "property uchar blue\n";
|
||
1080 | out << "end_header\n";
|
||
1081 | unsigned char r,g,b; |
||
1082 | float x, y, z ;
|
||
1083 | for(unsigned int i = 0; i < full_cloud.points.size() ; i++){ |
||
1084 | b = *( (unsigned char*)&(full_cloud.points[i].rgb)); |
||
1085 | g = *(1+(unsigned char*)&(full_cloud.points[i].rgb)); |
||
1086 | r = *(2+(unsigned char*)&(full_cloud.points[i].rgb)); |
||
1087 | x = full_cloud.points[i].x; |
||
1088 | y = full_cloud.points[i].y; |
||
1089 | z = full_cloud.points[i].z; |
||
1090 | out << qSetFieldWidth(8) << x << " " << y << " " << z << " "; |
||
1091 | out << qSetFieldWidth(3) << r << " " << g << " " << b << "\n"; |
||
1092 | } |
||
1093 | } |
||
1094 | |||
1095 | |||
1096 | void GraphManager::saveTrajectory(QString filebasename){
|
||
1097 | struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime); |
||
1098 | if(graph_.size() == 0){ |
||
1099 | ROS_ERROR("Graph is empty, no trajectory can be saved");
|
||
1100 | return;
|
||
1101 | } |
||
1102 | ROS_INFO("Logging Trajectory");
|
||
1103 | QMutexLocker locker(&optimizer_mutex); |
||
1104 | std::string gt = ParameterServer::instance()->get<std::string>("ground_truth_frame_name"); |
||
1105 | |||
1106 | ROS_INFO("Comparison of relative motion with ground truth");
|
||
1107 | QString gtt_fname("_ground_truth.txt");
|
||
1108 | QFile gtt_file(gtt_fname.prepend(filebasename));//file is closed on destruction
|
||
1109 | if(!gtt_file.open(QIODevice::WriteOnly|QIODevice::Text)) return; //TODO: Errormessage |
||
1110 | QTextStream gtt_out(>t_file); |
||
1111 | tf::StampedTransform b2p = graph_[0]->getGroundTruthTransform();
|
||
1112 | gtt_out.setRealNumberNotation(QTextStream::FixedNotation); |
||
1113 | gtt_out << "# TF Coordinate Frame ID: " << b2p.frame_id_.c_str() << "(data: " << b2p.child_frame_id_.c_str() << ")\n"; |
||
1114 | |||
1115 | |||
1116 | QString et_fname("_estimate.txt");
|
||
1117 | QFile et_file (et_fname.prepend(filebasename));//file is closed on destruction
|
||
1118 | if(!et_file.open(QIODevice::WriteOnly|QIODevice::Text)) return; //TODO: Errormessage |
||
1119 | QTextStream et_out(&et_file); |
||
1120 | et_out.setRealNumberNotation(QTextStream::FixedNotation); |
||
1121 | b2p = graph_[0]->getBase2PointsTransform();
|
||
1122 | et_out << "# TF Coordinate Frame ID: " << b2p.frame_id_.c_str() << "(data: " << b2p.child_frame_id_.c_str() << ")\n"; |
||
1123 | |||
1124 | for (unsigned int i = 0; i < optimizer_->vertices().size(); ++i) { |
||
1125 | g2o::VertexSE3* v = dynamic_cast<g2o::VertexSE3*>(optimizer_->vertex(i));
|
||
1126 | ROS_ERROR_COND(!v, "Nullpointer in graph at position %i!", i);
|
||
1127 | |||
1128 | tf::Transform pose = g2o2TF(v->estimate()); |
||
1129 | |||
1130 | tf::StampedTransform base2points = graph_[i]->getBase2PointsTransform();//get pose of base w.r.t current pc at capture time
|
||
1131 | tf::Transform world2base = init_base_pose_*base2points*pose*base2points.inverse(); |
||
1132 | |||
1133 | logTransform(et_out, world2base, graph_[i]->pc_col->header.stamp.toSec()); |
||
1134 | //Eigen::Matrix<double, 6,6> uncertainty = v->uncertainty();
|
||
1135 | //et_out << uncertainty(0,0) << "\t" << uncertainty(1,1) << "\t" << uncertainty(2,2) << "\t" << uncertainty(3,3) << "\t" << uncertainty(4,4) << "\t" << uncertainty(5,5) <<"\n" ;
|
||
1136 | if(!gt.empty()){
|
||
1137 | tf::StampedTransform gt_world2base = graph_[i ]->getGroundTruthTransform();//get mocap pose of base in map
|
||
1138 | if( gt_world2base.frame_id_ == "/missing_ground_truth" ){ |
||
1139 | ROS_WARN_STREAM("Skipping ground truth: " << gt_world2base.child_frame_id_ << " child/parent " << gt_world2base.frame_id_); |
||
1140 | continue;
|
||
1141 | } |
||
1142 | logTransform(gtt_out, gt_world2base, gt_world2base.stamp_.toSec()); |
||
1143 | //logTransform(et_out, world2base, gt_world2base.stamp_.toSec());
|
||
1144 | } |
||
1145 | } |
||
1146 | ROS_INFO_COND(!gt.empty(), "Written logfiles ground_truth_trajectory.txt and estimated_trajectory.txt");
|
||
1147 | ROS_INFO_COND(gt.empty(), "Written logfile estimated_trajectory.txt");
|
||
1148 | clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s"); |
||
1149 | } |
||
1150 | |||
1151 | |||
1152 | void GraphManager::sendAllClouds(){
|
||
1153 | struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime); |
||
1154 | if (batch_cloud_pub_.getNumSubscribers() == 0){ |
||
1155 | ROS_WARN("No Subscribers: Sending of clouds cancelled");
|
||
1156 | return;
|
||
1157 | } |
||
1158 | |||
1159 | ROS_INFO("Sending out all clouds");
|
||
1160 | batch_processing_runs_ = true;
|
||
1161 | ros::Rate r(5); //slow down a bit, to allow for transmitting to and processing in other nodes |
||
1162 | |||
1163 | for (unsigned int i = 0; i < optimizer_->vertices().size(); ++i) { |
||
1164 | g2o::VertexSE3* v = dynamic_cast<g2o::VertexSE3*>(optimizer_->vertex(i));
|
||
1165 | if(!v){
|
||
1166 | ROS_ERROR("Nullpointer in graph at position %i!", i);
|
||
1167 | continue;
|
||
1168 | } |
||
1169 | |||
1170 | tf::Transform base2points = graph_[i]->getBase2PointsTransform();//get pose of base w.r.t current pc at capture time
|
||
1171 | printTransform("base2points", base2points);
|
||
1172 | tf::Transform computed_motion = g2o2TF(v->estimate());//get pose of point cloud w.r.t. first frame's pc
|
||
1173 | printTransform("computed_motion", computed_motion);
|
||
1174 | printTransform("init_base_pose_", init_base_pose_);
|
||
1175 | |||
1176 | tf::Transform world2base = init_base_pose_*base2points*computed_motion*base2points.inverse(); |
||
1177 | tf::Transform gt_world2base = graph_[i]->getGroundTruthTransform();//get mocap pose of base in map
|
||
1178 | tf::Transform err = gt_world2base.inverseTimes(world2base); |
||
1179 | //TODO: Compute err from relative transformations betw. time steps
|
||
1180 | |||
1181 | ros::Time now = ros::Time::now(); //makes sure things have a corresponding timestamp
|
||
1182 | ROS_DEBUG("Sending out transform %i", i);
|
||
1183 | printTransform("World->Base", world2base);
|
||
1184 | std::string fixed_frame = ParameterServer::instance()->get<std::string>("fixed_frame_name"); |
||
1185 | br_.sendTransform(tf::StampedTransform(world2base, now, fixed_frame, "/openni_camera"));
|
||
1186 | br_.sendTransform(tf::StampedTransform(err, now, fixed_frame, "/where_mocap_should_be"));
|
||
1187 | ROS_DEBUG("Sending out cloud %i", i);
|
||
1188 | //graph_[i]->publish("/batch_transform", now, batch_cloud_pub_);
|
||
1189 | graph_[i]->publish("/openni_rgb_optical_frame", now, batch_cloud_pub_);
|
||
1190 | //tf::Transform ground_truth_tf = graph_[i]->getGroundTruthTransform();
|
||
1191 | r.sleep(); |
||
1192 | } |
||
1193 | |||
1194 | batch_processing_runs_ = false;
|
||
1195 | Q_EMIT sendFinished(); |
||
1196 | clock_gettime(CLOCK_MONOTONIC, &finish); elapsed = (finish.tv_sec - starttime.tv_sec); elapsed += (finish.tv_nsec - starttime.tv_nsec) / 1000000000.0; ROS_INFO_STREAM_COND_NAMED(elapsed > ParameterServer::instance()->get<double>("min_time_reported"), "timings", __FUNCTION__ << " runtime: "<< elapsed <<" s"); |
||
1197 | } |
||
1198 | |||
1199 | void GraphManager::setMaxDepth(float max_depth){ |
||
1200 | Max_Depth = max_depth; |
||
1201 | ROS_INFO("Max Depth set to: %f", max_depth);
|
||
1202 | } |
||
1203 | |||
1204 | void GraphManager::cloudRendered(pointcloud_type const * pc) { |
||
1205 | for(int i = graph_.size()-1; i >= 0; i--){ |
||
1206 | if(graph_[i]->pc_col.get() == pc){
|
||
1207 | graph_[i]->clearPointCloud(); |
||
1208 | ROS_WARN("Cleared PointCloud after rendering to openGL list. It will not be available for save/send.");
|
||
1209 | return;
|
||
1210 | } |
||
1211 | } |
||
1212 | } |
||
1213 | |||
1214 | |||
1215 | void GraphManager::sanityCheck(float thresh){ |
||
1216 | thresh *=thresh; //squaredNorm
|
||
1217 | QMutexLocker locker(&optimizer_mutex); |
||
1218 | EdgeSet::iterator edge_iter = optimizer_->edges().begin(); |
||
1219 | for(int i =0;edge_iter != optimizer_->edges().end(); edge_iter++, i++) { |
||
1220 | g2o::EdgeSE3* myedge = dynamic_cast<g2o::EdgeSE3*>(*edge_iter);
|
||
1221 | Eigen::Vector3d ev = myedge->measurement().translation(); |
||
1222 | if(ev.squaredNorm() > thresh){
|
||
1223 | optimizer_->removeEdge(myedge); |
||
1224 | } |
||
1225 | } |
||
1226 | } |
||
1227 | void GraphManager::pruneEdgesWithErrorAbove(float thresh){ |
||
1228 | QMutexLocker locker(&optimizer_mutex); |
||
1229 | EdgeSet::iterator edge_iter = optimizer_->edges().begin(); |
||
1230 | for(int i =0;edge_iter != optimizer_->edges().end(); edge_iter++, i++) { |
||
1231 | g2o::EdgeSE3* myedge = dynamic_cast<g2o::EdgeSE3*>(*edge_iter);
|
||
1232 | g2o::EdgeSE3::ErrorVector ev = myedge->error(); |
||
1233 | if(ev.squaredNorm() > thresh){
|
||
1234 | optimizer_->removeEdge(myedge); |
||
1235 | } |
||
1236 | } |
||
1237 | } |
||
1238 | void GraphManager::printEdgeErrors(QString filename){
|
||
1239 | QMutexLocker locker(&optimizer_mutex); |
||
1240 | std::fstream filestr; |
||
1241 | filestr.open (qPrintable(filename), std::fstream::out ); |
||
1242 | |||
1243 | EdgeSet::iterator edge_iter = optimizer_->edges().begin(); |
||
1244 | for(int i =0;edge_iter != optimizer_->edges().end(); edge_iter++, i++) { |
||
1245 | g2o::EdgeSE3* myedge = dynamic_cast<g2o::EdgeSE3*>(*edge_iter);
|
||
1246 | g2o::EdgeSE3::ErrorVector ev = myedge->error(); |
||
1247 | ROS_INFO_STREAM("Error Norm for edge " << i << ": " << ev.squaredNorm()); |
||
1248 | filestr << "Error for edge " << i << ": " << ev.squaredNorm() << std::endl; |
||
1249 | } |
||
1250 | filestr.close(); |
||
1251 | } |
||
1252 | |||
1253 | bool GraphManager::isBusy(){
|
||
1254 | return (batch_processing_runs_ || process_node_runs_ );
|
||
1255 | } |
||
1256 | |||
1257 | double GraphManager::geodesicDiscount(g2o::HyperDijkstra& hypdij, const MatchingResult& mr){ |
||
1258 | //Discount by geodesic distance to root node
|
||
1259 | const g2o::HyperDijkstra::AdjacencyMap am = hypdij.adjacencyMap();
|
||
1260 | g2o::VertexSE3* older_vertex = dynamic_cast<g2o::VertexSE3*>(optimizer_->vertex(mr.edge.id1));
|
||
1261 | double discount_factor = am.at(older_vertex).distance();
|
||
1262 | discount_factor = discount_factor > 0.0? 1.0/discount_factor : 1.0;//avoid inf |
||
1263 | ROS_INFO("Discount weight for connection to Node %i = %f", mr.edge.id1, discount_factor);
|
||
1264 | return discount_factor;
|
||
1265 | } |