Statistics
| Branch: | Revision:

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

History | View | Annotate | Download (61.8 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
#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, &chi;<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(&gtt_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
}