graph.cpp
Go to the documentation of this file.
00001 #include "constants.h"
00002 #include "graph.h"
00003 #include "cluster.h"
00004 #include "tools.h"
00005 
00006 using namespace tools;
00007 
00008 namespace slam
00009 {
00010 
00011   Graph::Graph(LoopClosing* loop_closing) : frame_id_(-1), loop_closing_(loop_closing)
00012   {
00013     init();
00014   }
00015 
00016   void Graph::init()
00017   {
00018     // Initialize the g2o graph optimizer
00019     g2o::BlockSolverX::LinearSolverType * linear_solver_ptr;
00020     linear_solver_ptr = new g2o::LinearSolverCholmod<g2o::BlockSolverX::PoseMatrixType>();
00021     g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linear_solver_ptr);
00022     g2o::OptimizationAlgorithmLevenberg * solver =
00023       new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
00024     graph_optimizer_.setAlgorithm(solver);
00025 
00026     // Remove locking file if exists
00027     string lock_file = WORKING_DIRECTORY + ".graph.lock";
00028     if (fs::exists(lock_file))
00029       remove(lock_file.c_str());
00030 
00031     // Advertise topics
00032     ros::NodeHandle nhp("~");
00033     pose_pub_ = nhp.advertise<nav_msgs::Odometry>("graph_camera_odometry", 1);
00034     graph_pub_ = nhp.advertise<stereo_slam::GraphPoses>("graph_poses", 2);
00035   }
00036 
00037   void Graph::run()
00038   {
00039     ros::Rate r(50);
00040     while(ros::ok())
00041     {
00042       if(checkNewFrameInQueue())
00043       {
00044         processNewFrame();
00045       }
00046       r.sleep();
00047     }
00048   }
00049 
00050   void Graph::addFrameToQueue(Frame frame)
00051   {
00052     mutex::scoped_lock lock(mutex_frame_queue_);
00053     frame_queue_.push_back(frame);
00054   }
00055 
00056   bool Graph::checkNewFrameInQueue()
00057   {
00058     mutex::scoped_lock lock(mutex_frame_queue_);
00059     return(!frame_queue_.empty());
00060   }
00061 
00062   void Graph::processNewFrame()
00063   {
00064     // Get the frame
00065     Frame frame;
00066     {
00067       mutex::scoped_lock lock(mutex_frame_queue_);
00068       frame = frame_queue_.front();
00069       frame_queue_.pop_front();
00070     }
00071 
00072     // The clusters of this frame
00073     vector< vector<int> > clusters = frame.getClusters();
00074 
00075     // Frame id
00076     frame_id_ = frame.getId();
00077 
00078     // Save the frame
00079     saveFrame(frame);
00080 
00081     // Save the frame timestamp
00082     frame_stamps_.push_back(frame.getTimestamp());
00083 
00084     // Extract sift
00085     cv::Mat sift_desc = frame.computeSift();
00086 
00087     // Loop of frame clusters
00088     vector<int> vertex_ids;
00089     vector<Cluster> clusters_to_close_loop;
00090     vector<Eigen::Vector4f> cluster_centroids = frame.getClusterCentroids();
00091     vector<cv::Point3f> points = frame.getCameraPoints();
00092     vector<cv::KeyPoint> kp_l = frame.getLeftKp();
00093     vector<cv::KeyPoint> kp_r = frame.getRightKp();
00094     tf::Transform camera_pose = frame.getCameraPose();
00095     cv::Mat orb_desc = frame.getLeftDesc();
00096     for (uint i=0; i<clusters.size(); i++)
00097     {
00098       // Correct cluster pose with the last graph update
00099       tf::Transform cluster_pose = Tools::transformVector4f(cluster_centroids[i], camera_pose);
00100       initial_cluster_pose_history_.push_back(cluster_pose);
00101 
00102       // Add cluster to the graph
00103       int id = addVertex(cluster_pose);
00104 
00105       // Store information
00106       cluster_frame_relation_.push_back( make_pair(id, frame_id_) );
00107       local_cluster_poses_.push_back( Tools::vector4fToTransform(cluster_centroids[i]) );
00108       vertex_ids.push_back(id);
00109 
00110       // Build cluster
00111       cv::Mat c_desc_orb, c_desc_sift;
00112       vector<cv::KeyPoint> c_kp_l, c_kp_r;
00113       vector<cv::Point3f> c_points;
00114       for (uint j=0; j<clusters[i].size(); j++)
00115       {
00116         int idx = clusters[i][j];
00117         c_kp_l.push_back(kp_l[idx]);
00118         c_kp_r.push_back(kp_r[idx]);
00119         c_points.push_back(points[idx]);
00120         c_desc_orb.push_back(orb_desc.row(idx));
00121         c_desc_sift.push_back(sift_desc.row(idx));
00122       }
00123       Cluster cluster(id, frame_id_, camera_pose, c_kp_l, c_kp_r, c_desc_orb, c_desc_sift, c_points);
00124       clusters_to_close_loop.push_back(cluster);
00125     }
00126 
00127     // Send the new clusters to the loop closing thread
00128     for (uint i=0; i<clusters_to_close_loop.size(); i++)
00129       loop_closing_->addClusterToQueue(clusters_to_close_loop[i]);
00130 
00131     // Add edges between clusters of the same frame
00132     if (clusters.size() > 1)
00133     {
00134       // Retrieve all possible combinations
00135       vector< vector<int> > combinations = createComb(vertex_ids);
00136 
00137       for (uint i=0; i<combinations.size(); i++)
00138       {
00139         int id_a = vertex_ids[combinations[i][0]];
00140         int id_b = vertex_ids[combinations[i][1]];
00141 
00142         tf::Transform pose_a = getVertexPose(id_a);
00143         tf::Transform pose_b = getVertexPose(id_b);
00144 
00145         tf::Transform edge = pose_a.inverse() * pose_b;
00146         addEdge(id_a, id_b, edge, LC_MAX_INLIERS);
00147       }
00148     }
00149 
00150     // Connect this frame with the previous
00151     if (frame_id_ > 0)
00152     {
00153       vector<int> prev_frame_vertices;
00154       getFrameVertices(frame_id_ - 1, prev_frame_vertices);
00155 
00156       // Connect only the closest vertices between the two frames
00157       double min_dist = DBL_MAX;
00158       vector<int> closest_vertices;
00159       vector<tf::Transform> closest_poses;
00160       for (uint i=0; i<vertex_ids.size(); i++)
00161       {
00162         // The pose of this vertex
00163         tf::Transform cur_vertex_pose = getVertexPose(vertex_ids[i]);
00164 
00165         for (uint j=0; j<prev_frame_vertices.size(); j++)
00166         {
00167           tf::Transform prev_vertex_pose = getVertexPose(prev_frame_vertices[j]);
00168 
00169           double dist = Tools::poseDiff3D(cur_vertex_pose, prev_vertex_pose);
00170           if (dist < min_dist)
00171           {
00172             closest_vertices.clear();
00173             closest_vertices.push_back(vertex_ids[i]);
00174             closest_vertices.push_back(prev_frame_vertices[j]);
00175 
00176             closest_poses.clear();
00177             closest_poses.push_back(cur_vertex_pose);
00178             closest_poses.push_back(prev_vertex_pose);
00179             min_dist = dist;
00180           }
00181         }
00182       }
00183 
00184       if (closest_vertices.size() > 0)
00185       {
00186         tf::Transform edge = closest_poses[0].inverse() * closest_poses[1];
00187         addEdge(closest_vertices[0], closest_vertices[1], edge, frame.getInliersNumWithPreviousFrame());
00188       }
00189       else
00190         ROS_ERROR("[Localization:] Impossible to connect current and previous frame. Graph will have non-connected parts!");
00191     }
00192 
00193     // Save graph to file
00194     saveGraph();
00195 
00196     // Publish the graph
00197     publishGraph();
00198 
00199     // Publish camera pose
00200     int last_idx = -1;
00201     {
00202       mutex::scoped_lock lock(mutex_graph_);
00203       last_idx = graph_optimizer_.vertices().size() - 1;
00204     }
00205     tf::Transform updated_camera_pose = getVertexCameraPose(last_idx, true);
00206     publishCameraPose(updated_camera_pose);
00207   }
00208 
00209   tf::Transform Graph::correctClusterPose(tf::Transform initial_pose)
00210   {
00211     // Get last
00212     int last_idx = -1;
00213     {
00214       mutex::scoped_lock lock(mutex_graph_);
00215       last_idx = graph_optimizer_.vertices().size() - 1;
00216     }
00217 
00218     if (initial_cluster_pose_history_.size() > 0 && last_idx >= 0)
00219     {
00220       tf::Transform last_graph_pose = getVertexPose(last_idx);
00221       tf::Transform last_graph_initial = initial_cluster_pose_history_.at(last_idx);
00222       tf::Transform diff = last_graph_initial.inverse() * initial_pose;
00223 
00224       // Compute the corrected pose
00225       return last_graph_pose * diff;
00226     }
00227     else
00228       return initial_pose;
00229   }
00230 
00231   vector< vector<int> > Graph::createComb(vector<int> cluster_ids)
00232   {
00233     string bitmask(2, 1);
00234     bitmask.resize(cluster_ids.size(), 0);
00235 
00236     vector<int> comb;
00237     vector< vector<int> > combinations;
00238     do {
00239       for (uint i = 0; i < cluster_ids.size(); ++i)
00240       {
00241         if (bitmask[i]) comb.push_back(i);
00242       }
00243       combinations.push_back(comb);
00244       comb.clear();
00245     } while (prev_permutation(bitmask.begin(), bitmask.end()));
00246 
00247     return combinations;
00248   }
00249 
00250   int Graph::addVertex(tf::Transform pose)
00251   {
00252     mutex::scoped_lock lock(mutex_graph_);
00253 
00254     // Convert pose for graph
00255     Eigen::Isometry3d vertex_pose = Tools::tfToIsometry(pose);
00256 
00257     // Set node id equal to graph size
00258     int id = graph_optimizer_.vertices().size();
00259 
00260     // Build the vertex
00261     g2o::VertexSE3* cur_vertex = new g2o::VertexSE3();
00262     cur_vertex->setId(id);
00263     cur_vertex->setEstimate(vertex_pose);
00264     if (id == 0)
00265     {
00266       // First time, no edges.
00267       cur_vertex->setFixed(true);
00268     }
00269     graph_optimizer_.addVertex(cur_vertex);
00270     return id;
00271   }
00272 
00273   void Graph::addEdge(int i, int j, tf::Transform edge, int sigma)
00274   {
00275     mutex::scoped_lock lock(mutex_graph_);
00276 
00277     // Sanity check
00278     if (sigma > LC_MAX_INLIERS)
00279       sigma = LC_MAX_INLIERS;
00280 
00281     // Get the vertices
00282     g2o::VertexSE3* v_i = dynamic_cast<g2o::VertexSE3*>(graph_optimizer_.vertices()[i]);
00283     g2o::VertexSE3* v_j = dynamic_cast<g2o::VertexSE3*>(graph_optimizer_.vertices()[j]);
00284 
00285     Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity() * (double)sigma;
00286 
00287     // Add the new edge to graph
00288     g2o::EdgeSE3* e = new g2o::EdgeSE3();
00289     Eigen::Isometry3d t = Tools::tfToIsometry(edge);
00290     e->setVertex(0, v_i);
00291     e->setVertex(1, v_j);
00292     e->setMeasurement(t);
00293     e->setInformation(information);
00294     graph_optimizer_.addEdge(e);
00295   }
00296 
00297   void Graph::update()
00298   {
00299     mutex::scoped_lock lock(mutex_graph_);
00300 
00301     // Optimize!
00302     graph_optimizer_.initializeOptimization();
00303     graph_optimizer_.optimize(20);
00304 
00305     ROS_INFO_STREAM("[Localization:] Optimization done in graph with " << graph_optimizer_.vertices().size() << " vertices.");
00306   }
00307 
00308   void Graph::findClosestVertices(int vertex_id, int window_center, int window, int best_n, vector<int> &neighbors)
00309   {
00310     // Init
00311     neighbors.clear();
00312     tf::Transform vertex_pose = getVertexPose(vertex_id);
00313 
00314     // Loop thought all the other nodes
00315     vector< pair< int,double > > neighbor_distances;
00316     for (uint i=0; i<graph_optimizer_.vertices().size(); i++)
00317     {
00318       if ( (int)i == vertex_id ) continue;
00319       if ((int)i > window_center-window && (int)i < window_center+window) continue;
00320 
00321       // Get the node pose
00322       tf::Transform cur_pose = getVertexPose(i);
00323       double dist = Tools::poseDiff2D(cur_pose, vertex_pose);
00324       neighbor_distances.push_back(make_pair(i, dist));
00325     }
00326 
00327     // Exit if no neighbors
00328     if (neighbor_distances.size() == 0) return;
00329 
00330     // Sort the neighbors
00331     sort(neighbor_distances.begin(), neighbor_distances.end(), Tools::sortByDistance);
00332 
00333     // Min number
00334     if ((int)neighbor_distances.size() < best_n)
00335       best_n = neighbor_distances.size();
00336 
00337     for (int i=0; i<best_n; i++)
00338       neighbors.push_back(neighbor_distances[i].first);
00339   }
00340 
00341   void Graph::getFrameVertices(int frame_id, vector<int> &vertices)
00342   {
00343     vertices.clear();
00344     for (uint i=0; i<cluster_frame_relation_.size(); i++)
00345     {
00346       if (cluster_frame_relation_[i].second == frame_id)
00347         vertices.push_back(cluster_frame_relation_[i].first);
00348     }
00349   }
00350 
00351   int Graph::getVertexFrameId(int id)
00352   {
00353     int frame_id = -1;
00354     for (uint i=0; i<cluster_frame_relation_.size(); i++)
00355     {
00356       if (cluster_frame_relation_[i].first == id)
00357       {
00358         frame_id = cluster_frame_relation_[i].second;
00359         break;
00360       }
00361     }
00362     return frame_id;
00363   }
00364 
00365   int Graph::getLastVertexFrameId()
00366   {
00367     // Get last
00368     int last_idx = -1;
00369     {
00370       mutex::scoped_lock lock(mutex_graph_);
00371       last_idx = graph_optimizer_.vertices().size() - 1;
00372     }
00373 
00374     return getVertexFrameId(last_idx);
00375   }
00376 
00377 
00378   tf::Transform Graph::getVertexPose(int id, bool lock)
00379   {
00380     if (lock)
00381     {
00382       mutex::scoped_lock lock(mutex_graph_);
00383 
00384       if( id >= 0)
00385       {
00386         g2o::VertexSE3* vertex =  dynamic_cast<g2o::VertexSE3*>(graph_optimizer_.vertices()[id]);
00387         return Tools::getVertexPose(vertex);
00388       }
00389       else
00390       {
00391         tf::Transform tmp;
00392         tmp.setIdentity();
00393         return tmp;
00394       }
00395     }
00396     else
00397     {
00398       if( id >= 0)
00399       {
00400         g2o::VertexSE3* vertex =  dynamic_cast<g2o::VertexSE3*>(graph_optimizer_.vertices()[id]);
00401         return Tools::getVertexPose(vertex);
00402       }
00403       else
00404       {
00405         tf::Transform tmp;
00406         tmp.setIdentity();
00407         return tmp;
00408       }
00409     }
00410   }
00411 
00412   bool Graph::getFramePose(int frame_id, tf::Transform& frame_pose)
00413   {
00414     frame_pose.setIdentity();
00415     vector<int> frame_vertices;
00416     getFrameVertices(frame_id, frame_vertices);
00417 
00418     if (frame_vertices.size() == 0)
00419     {
00420       return false;
00421     }
00422     else
00423     {
00424       frame_pose = getVertexCameraPose(frame_vertices[0]);
00425       return true;
00426     }
00427   }
00428 
00429   tf::Transform Graph::getVertexPoseRelativeToCamera(int id)
00430   {
00431     return local_cluster_poses_[id];
00432   }
00433 
00434   tf::Transform Graph::getVertexCameraPose(int id, bool lock)
00435   {
00436     tf::Transform vertex_pose = getVertexPose(id, lock);
00437     return vertex_pose * local_cluster_poses_[id].inverse();
00438   }
00439 
00440   void Graph::saveFrame(Frame frame, bool draw_clusters)
00441   {
00442     cv::Mat img;
00443     frame.getLeftImg().copyTo(img);
00444     if (img.cols == 0)
00445       return;
00446 
00447     if (draw_clusters)
00448     {
00449       // Draw the clusters
00450       vector< vector<int> > clusters = frame.getClusters();
00451       vector<cv::KeyPoint> kp = frame.getLeftKp();
00452       cv::RNG rng(12345);
00453       for (uint i=0; i<clusters.size(); i++)
00454       {
00455         cv::Scalar color = cv::Scalar(rng.uniform(0,255), rng.uniform(0, 255), rng.uniform(0, 255));
00456         for (uint j=0; j<clusters[i].size(); j++)
00457           cv::circle(img, kp[clusters[i][j]].pt, 5, color, -1);
00458       }
00459     }
00460 
00461     // Save
00462     string frame_id_str = Tools::convertTo5digits(frame.getId());
00463     string keyframe_file = WORKING_DIRECTORY + "keyframes/" + frame_id_str + ".jpg";
00464     cv::imwrite( keyframe_file, img );
00465   }
00466 
00467   void Graph::saveGraph()
00468   {
00469     string lock_file, vertices_file, edges_file;
00470     vertices_file = WORKING_DIRECTORY + "graph_vertices.txt";
00471     edges_file = WORKING_DIRECTORY + "graph_edges.txt";
00472     lock_file = WORKING_DIRECTORY + ".graph.lock";
00473 
00474     // Wait until lock file has been released
00475     while(fs::exists(lock_file));
00476 
00477     // Create a locking element
00478     fstream f_lock(lock_file.c_str(), ios::out | ios::trunc);
00479 
00480     // Open to append
00481     fstream f_vertices(vertices_file.c_str(), ios::out | ios::trunc);
00482     fstream f_edges(edges_file.c_str(), ios::out | ios::trunc);
00483 
00484     mutex::scoped_lock lock(mutex_graph_);
00485 
00486     vector<int> processed_frames;
00487 
00488     // Output the vertices file
00489     for (uint i=0; i<graph_optimizer_.vertices().size(); i++)
00490     {
00491       // Is this frame processed?
00492       bool found = false;
00493       int id = getVertexFrameId(i);
00494       for (uint j=0; j<processed_frames.size(); j++)
00495       {
00496         if (processed_frames[j] == id)
00497         {
00498           found = true;
00499           break;
00500         }
00501       }
00502       if (found) continue;
00503       processed_frames.push_back(id);
00504 
00505       tf::Transform pose = getVertexCameraPose(i, false);//*camera2odom_;
00506       f_vertices << fixed <<
00507         setprecision(6) <<
00508         frame_stamps_[id] << "," <<
00509         id << "," <<
00510         pose.getOrigin().x() << "," <<
00511         pose.getOrigin().y() << "," <<
00512         pose.getOrigin().z() << "," <<
00513         pose.getRotation().x() << "," <<
00514         pose.getRotation().y() << "," <<
00515         pose.getRotation().z() << "," <<
00516         pose.getRotation().w() <<  endl;
00517     }
00518     f_vertices.close();
00519 
00520     // Output the edges file
00521     for ( g2o::OptimizableGraph::EdgeSet::iterator it=graph_optimizer_.edges().begin();
00522         it!=graph_optimizer_.edges().end(); it++)
00523     {
00524       g2o::EdgeSE3* e = dynamic_cast<g2o::EdgeSE3*> (*it);
00525       if (e)
00526       {
00527         // Get the frames corresponding to these edges
00528         int frame_a = Graph::getVertexFrameId(e->vertices()[0]->id());
00529         int frame_b = Graph::getVertexFrameId(e->vertices()[1]->id());
00530 
00531         if (abs(frame_a - frame_b) > 1 )
00532         {
00533 
00534           tf::Transform pose_0 = getVertexCameraPose(e->vertices()[0]->id(), false);//*camera2odom_;
00535           tf::Transform pose_1 = getVertexCameraPose(e->vertices()[1]->id(), false);//*camera2odom_;
00536 
00537           // Extract the inliers
00538           Eigen::Matrix<double, 6, 6> information = e->information();
00539           int inliers = 0;
00540           if (information(0,0) > 0.0001)
00541             inliers = (int)information(0,0);
00542 
00543           // Write
00544           f_edges <<
00545             e->vertices()[0]->id() << "," <<
00546             e->vertices()[1]->id() << "," <<
00547             inliers << "," <<
00548             setprecision(6) <<
00549             pose_0.getOrigin().x() << "," <<
00550             pose_0.getOrigin().y() << "," <<
00551             pose_0.getOrigin().z() << "," <<
00552             pose_0.getRotation().x() << "," <<
00553             pose_0.getRotation().y() << "," <<
00554             pose_0.getRotation().z() << "," <<
00555             pose_0.getRotation().w() << "," <<
00556             pose_1.getOrigin().x() << "," <<
00557             pose_1.getOrigin().y() << "," <<
00558             pose_1.getOrigin().z() << "," <<
00559             pose_1.getRotation().x() << "," <<
00560             pose_1.getRotation().y() << "," <<
00561             pose_1.getRotation().z() << "," <<
00562             pose_1.getRotation().w() << endl;
00563         }
00564       }
00565     }
00566     f_edges.close();
00567 
00568     // Un-lock
00569     f_lock.close();
00570     int ret_code = remove(lock_file.c_str());
00571     if (ret_code != 0)
00572       ROS_ERROR("[Localization:] Error deleting the locking file.");
00573   }
00574 
00575   void Graph::publishCameraPose(tf::Transform camera_pose)
00576   {
00577     if (pose_pub_.getNumSubscribers() > 0)
00578     {
00579       nav_msgs::Odometry pose_msg;
00580       pose_msg.header.stamp = ros::Time::now();
00581       tf::poseTFToMsg(camera_pose, pose_msg.pose.pose);
00582       pose_pub_.publish(pose_msg);
00583     }
00584   }
00585 
00586   void Graph::publishGraph()
00587   {
00588     if (graph_pub_.getNumSubscribers() > 0)
00589     {
00590       mutex::scoped_lock lock(mutex_graph_);
00591 
00592       // Build the graph data
00593       vector<int> ids;
00594       vector<double> x, y, z, qx, qy, qz, qw;
00595       vector<int> processed_frames;
00596       for (uint i=0; i<graph_optimizer_.vertices().size(); i++)
00597       {
00598         bool found = false;
00599         int id = getVertexFrameId(i);
00600         for (uint j=0; j<processed_frames.size(); j++)
00601         {
00602           if (processed_frames[j] == id)
00603           {
00604             found = true;
00605             break;
00606           }
00607         }
00608         if (found) continue;
00609         processed_frames.push_back(id);
00610 
00611         tf::Transform pose = getVertexCameraPose(i, false);
00612         ids.push_back(id);
00613         x.push_back(pose.getOrigin().x());
00614         y.push_back(pose.getOrigin().y());
00615         z.push_back(pose.getOrigin().z());
00616         qx.push_back(pose.getRotation().x());
00617         qy.push_back(pose.getRotation().y());
00618         qz.push_back(pose.getRotation().z());
00619         qw.push_back(pose.getRotation().w());
00620       }
00621 
00622       // Publish
00623       stereo_slam::GraphPoses graph_msg;
00624       graph_msg.header.stamp = ros::Time::now();
00625       graph_msg.id = ids;
00626       graph_msg.x = x;
00627       graph_msg.y = y;
00628       graph_msg.z = z;
00629       graph_msg.qx = qx;
00630       graph_msg.qy = qy;
00631       graph_msg.qz = qz;
00632       graph_msg.qw = qw;
00633       graph_pub_.publish(graph_msg);
00634     }
00635   }
00636 
00637 } //namespace slam


stereo_slam
Author(s): Pep Lluis Negre
autogenerated on Thu Jun 6 2019 21:40:57