loop_closing.cpp
Go to the documentation of this file.
00001 #include <std_msgs/String.h>
00002 #include <image_geometry/pinhole_camera_model.h>
00003 
00004 #include <numeric>
00005 
00006 #include "loop_closing.h"
00007 #include "tools.h"
00008 
00009 using namespace tools;
00010 
00011 namespace slam
00012 {
00013 
00014   LoopClosing::LoopClosing()
00015   {
00016     ros::NodeHandle nhp("~");
00017     pub_num_keyframes_ = nhp.advertise<std_msgs::String>("keyframes", 2, true);
00018     pub_num_lc_ = nhp.advertise<std_msgs::String>("loop_closings", 2, true);
00019     pub_queue_ = nhp.advertise<std_msgs::String>("loop_closing_queue", 2, true);
00020     pub_lc_matchings_ = nhp.advertise<sensor_msgs::Image>("loop_closing_matchings", 2, true);
00021   }
00022 
00023   void LoopClosing::run()
00024   {
00025     // Init
00026     execution_dir_ = WORKING_DIRECTORY + "haloc";
00027     if (fs::is_directory(execution_dir_))
00028       fs::remove_all(execution_dir_);
00029     fs::path dir1(execution_dir_);
00030     if (!fs::create_directory(dir1))
00031       ROS_ERROR("[Localization:] ERROR -> Impossible to create the loop_closing directory.");
00032 
00033     loop_closures_dir_ = WORKING_DIRECTORY + "loop_closures";
00034     if (fs::is_directory(loop_closures_dir_))
00035       fs::remove_all(loop_closures_dir_);
00036     fs::path dir2(loop_closures_dir_);
00037     if (!fs::create_directory(dir2))
00038       ROS_ERROR("[Localization:] ERROR -> Impossible to create the loop_closing directory.");
00039 
00040     camera_model_ = graph_->getCameraModel();
00041     num_loop_closures_ = 0;
00042 
00043     // Publish first loop closure image
00044     stringstream s;
00045     s << " No Loop Closures ";
00046     cv::Mat no_loops = cv::Mat(384, 512, CV_8UC3);
00047     no_loops.rowRange(0, no_loops.rows).setTo(cv::Scalar(0,0,0));
00048     cv::putText(no_loops, s.str(), cv::Point(95, 200), cv::FONT_HERSHEY_PLAIN, 2, cv::Scalar(255,255,255), 2, 8);
00049     cv_bridge::CvImage ros_image;
00050     ros_image.image = no_loops.clone();
00051     ros_image.header.stamp = ros::Time::now();
00052     ros_image.encoding = "bgr8";
00053     pub_lc_matchings_.publish(ros_image.toImageMsg());
00054 
00055     // Loop
00056     ros::Rate r(50);
00057     while(ros::ok())
00058     {
00059 
00060       // Process new frame
00061       if(checkNewClusterInQueue())
00062       {
00063         processNewCluster();
00064 
00065         searchByProximity();
00066 
00067         searchByHash();
00068 
00069         // Publish loop closing information
00070         if (pub_num_keyframes_.getNumSubscribers() > 0)
00071         {
00072           std_msgs::String msg;
00073           msg.data = lexical_cast<string>(graph_->getFrameNum());
00074           pub_num_keyframes_.publish(msg);
00075         }
00076         if (pub_num_lc_.getNumSubscribers() > 0)
00077         {
00078           std_msgs::String msg;
00079           msg.data = lexical_cast<string>(cluster_lc_found_.size());
00080           pub_num_lc_.publish(msg);
00081         }
00082         if (pub_queue_.getNumSubscribers() > 0)
00083         {
00084           mutex::scoped_lock lock(mutex_cluster_queue_);
00085           std_msgs::String msg;
00086           msg.data = lexical_cast<string>(cluster_queue_.size());
00087           pub_queue_.publish(msg);
00088         }
00089       }
00090 
00091       r.sleep();
00092     }
00093   }
00094 
00095   void LoopClosing::addClusterToQueue(Cluster cluster)
00096   {
00097     mutex::scoped_lock lock(mutex_cluster_queue_);
00098     cluster_queue_.push_back(cluster);
00099   }
00100 
00101   bool LoopClosing::checkNewClusterInQueue()
00102   {
00103     mutex::scoped_lock lock(mutex_cluster_queue_);
00104     return(!cluster_queue_.empty());
00105   }
00106 
00107   void LoopClosing::processNewCluster()
00108   {
00109     // Get the cluster
00110     {
00111       mutex::scoped_lock lock(mutex_cluster_queue_);
00112       c_cluster_ = cluster_queue_.front();
00113       cluster_queue_.pop_front();
00114     }
00115 
00116     // Initialize hash
00117     if (!hash_.isInitialized())
00118       hash_.init(c_cluster_.getSift());
00119 
00120     // Save hash to table
00121     hash_table_.push_back(make_pair(c_cluster_.getId(), hash_.getHash(c_cluster_.getSift())));
00122 
00123     // Store
00124     cv::FileStorage fs(execution_dir_+"/"+lexical_cast<string>(c_cluster_.getId())+".yml", cv::FileStorage::WRITE);
00125     write(fs, "frame_id", c_cluster_.getFrameId());
00126     write(fs, "kp_l", c_cluster_.getLeftKp());
00127     write(fs, "kp_r", c_cluster_.getRightKp());
00128     write(fs, "desc", c_cluster_.getOrb());
00129     write(fs, "points", c_cluster_.getPoints());
00130     fs.release();
00131   }
00132 
00133   void LoopClosing::searchByProximity()
00134   {
00135     vector<int> cand_neighbors;
00136     graph_->findClosestVertices(c_cluster_.getId(), c_cluster_.getId(), LC_DISCARD_WINDOW, 3, cand_neighbors);
00137     for (uint i=0; i<cand_neighbors.size(); i++)
00138     {
00139       Cluster candidate = readCluster(cand_neighbors[i]);
00140       if (candidate.getOrb().rows == 0)
00141         continue;
00142 
00143       bool valid = closeLoopWithCluster(candidate);
00144 
00145       if (valid)
00146         ROS_INFO("[Localization:] Loop closure by proximity.");
00147     }
00148   }
00149 
00150   void LoopClosing::searchByHash()
00151   {
00152     // Get the candidates to close loop
00153     vector< pair<int,float> > hash_matching;
00154     getCandidates(c_cluster_.getId(), hash_matching);
00155     if (hash_matching.size() == 0) return;
00156 
00157     // Loop over candidates
00158     for (uint i=0; i<hash_matching.size(); i++)
00159     {
00160       Cluster candidate = readCluster(hash_matching[i].first);
00161       if (candidate.getOrb().rows == 0)
00162         continue;
00163 
00164       bool valid = closeLoopWithCluster(candidate);
00165 
00166       if (valid)
00167       {
00168         ROS_INFO("[Localization:] Loop closure by hash.");
00169         break;
00170       }
00171     }
00172   }
00173 
00174   bool LoopClosing::closeLoopWithCluster(Cluster candidate)
00175   {
00176     // Init
00177     const float matching_th = 0.8;
00178 
00179     // Descriptor matching
00180     vector<cv::DMatch> matches_1;
00181     Tools::ratioMatching(c_cluster_.getOrb(), candidate.getOrb(), matching_th, matches_1);
00182 
00183     // Compute the percentage of matchings
00184     int size_c = c_cluster_.getOrb().rows;
00185     int size_n = candidate.getOrb().rows;
00186     int m_percentage = round(100.0 * (float) matches_1.size() / (float) min(size_c, size_n) );
00187 
00188     // Get the neighbor clusters if enough matching percentage
00189     if (m_percentage > 35)
00190     {
00191       // Init accumulated data
00192       vector<int> cluster_query_list;
00193       vector<int> cluster_cand_list;
00194 
00195       // Candidate data
00196       cv::Mat all_cand_desc = candidate.getOrb();
00197       vector<cv::Point3f> all_cand_points = candidate.getWorldPoints();
00198       vector<cv::KeyPoint> all_cand_kp_l = candidate.getLeftKp();
00199       vector<cv::KeyPoint> all_cand_kp_r = candidate.getRightKp();
00200 
00201       // Query data
00202       cv::Mat all_query_desc = c_cluster_.getOrb();
00203       vector<cv::KeyPoint> all_query_kp_l = c_cluster_.getLeftKp();
00204       vector<cv::KeyPoint> all_query_kp_r = c_cluster_.getRightKp();
00205 
00206       // Init the cluster candidate list
00207       for (uint j=0; j<all_cand_points.size(); j++)
00208         cluster_cand_list.push_back(candidate.getId());
00209 
00210       // Increase the probability to close loop by extracting the candidate neighbors
00211       vector<int> cand_neighbors;
00212       graph_->findClosestVertices(candidate.getId(), c_cluster_.getId(), LC_DISCARD_WINDOW, LC_NEIGHBORS, cand_neighbors);
00213       for (uint j=0; j<cand_neighbors.size(); j++)
00214       {
00215         Cluster cand_neighbor = readCluster(cand_neighbors[j]);
00216         cv::Mat c_n_desc = cand_neighbor.getOrb();
00217         if (c_n_desc.rows == 0) continue;
00218 
00219         vector<cv::Point3f> points_tmp = cand_neighbor.getWorldPoints();
00220         vector<cv::KeyPoint> kp_tmp_l = cand_neighbor.getLeftKp();
00221         vector<cv::KeyPoint> kp_tmp_r = cand_neighbor.getRightKp();
00222 
00223         // Concatenate descriptors and points
00224         cv::vconcat(all_cand_desc, c_n_desc, all_cand_desc);
00225         all_cand_points.insert(all_cand_points.end(), points_tmp.begin(), points_tmp.end());
00226         all_cand_kp_l.insert(all_cand_kp_l.end(), kp_tmp_l.begin(), kp_tmp_l.end());
00227         all_cand_kp_r.insert(all_cand_kp_r.end(), kp_tmp_r.begin(), kp_tmp_r.end());
00228 
00229         // Save the cluster id for these points
00230         for (uint n=0; n<points_tmp.size(); n++)
00231           cluster_cand_list.push_back(cand_neighbor.getId());
00232       }
00233 
00234       // Init the cluster frame list
00235       for (uint j=0; j<c_cluster_.getLeftKp().size(); j++)
00236         cluster_query_list.push_back(c_cluster_.getId());
00237 
00238       // Extract all the clusters corresponding to the current cluster frame
00239       vector<int> query_clusters;
00240       graph_->getFrameVertices(c_cluster_.getFrameId(), query_clusters);
00241       for (uint j=0; j<query_clusters.size(); j++)
00242       {
00243         if (query_clusters[j] == c_cluster_.getId())
00244           continue;
00245 
00246         Cluster query_cluster = readCluster(query_clusters[j]);
00247         cv::Mat f_n_desc = query_cluster.getOrb();
00248         if (f_n_desc.rows == 0) continue;
00249 
00250         // Concatenate descriptors and keypoints
00251         cv::vconcat(all_query_desc, f_n_desc, all_query_desc);
00252         vector<cv::KeyPoint> query_n_kp_l = query_cluster.getLeftKp();
00253         vector<cv::KeyPoint> query_n_kp_r = query_cluster.getRightKp();
00254         all_query_kp_l.insert(all_query_kp_l.end(), query_n_kp_l.begin(), query_n_kp_l.end());
00255         all_query_kp_r.insert(all_query_kp_r.end(), query_n_kp_r.begin(), query_n_kp_r.end());
00256 
00257         // Save the cluster id for these points
00258         for (uint n=0; n<query_n_kp_l.size(); n++)
00259           cluster_query_list.push_back(query_cluster.getId());
00260       }
00261 
00262       // Match current frame descriptors with all the clusters
00263       vector<cv::DMatch> matches_2;
00264       Tools::ratioMatching(all_query_desc, all_cand_desc, matching_th, matches_2);
00265 
00266       if (matches_2.size() >= LC_MIN_INLIERS)
00267       {
00268         // Store matchings
00269         vector<int> query_matchings;
00270         vector<int> cand_matchings;
00271         vector<cv::Point2f> matched_query_kp_l, matched_query_kp_r;
00272         vector<cv::Point2f> matched_cand_kp_l, matched_cand_kp_r;
00273         vector<cv::Point3f> matched_cand_3d_points;
00274         for(uint j=0; j<matches_2.size(); j++)
00275         {
00276           // Features
00277           matched_query_kp_l.push_back(all_query_kp_l[matches_2[j].queryIdx].pt);
00278           matched_query_kp_r.push_back(all_query_kp_r[matches_2[j].queryIdx].pt);
00279           matched_cand_kp_l.push_back(all_cand_kp_l[matches_2[j].trainIdx].pt);
00280           matched_cand_kp_r.push_back(all_cand_kp_r[matches_2[j].trainIdx].pt);
00281 
00282           // 3d
00283           matched_cand_3d_points.push_back(all_cand_points[matches_2[j].trainIdx]);
00284 
00285           // Ids
00286           query_matchings.push_back(cluster_query_list[matches_2[j].queryIdx]);
00287           cand_matchings.push_back(cluster_cand_list[matches_2[j].trainIdx]);
00288         }
00289 
00290         // Estimate the motion
00291         vector<int> inliers;
00292         cv::Mat rvec, tvec;
00293         cv::solvePnPRansac(matched_cand_3d_points, matched_query_kp_l, graph_->getCameraMatrix(),
00294             cv::Mat(), rvec, tvec, false,
00295             100, 4.0, LC_MAX_INLIERS, inliers);
00296 
00297         // Loop found!
00298         if (inliers.size() >= LC_MIN_INLIERS)
00299         {
00300           tf::Transform estimated_transform = Tools::buildTransformation(rvec, tvec);
00301           estimated_transform = estimated_transform.inverse();
00302 
00303           // Get the inliers per cluster pair
00304           vector< vector<int> > cluster_pairs;
00305           vector<int> inliers_per_pair;
00306           vector<int> cand_kfs;
00307           for (uint i=0; i<inliers.size(); i++)
00308           {
00309             int query_cluster = query_matchings[inliers[i]];
00310             int cand_cluster = cand_matchings[inliers[i]];
00311             int cand_kf = graph_->getVertexFrameId(cand_cluster);
00312 
00313             // Build the unique candidate keyframes vector
00314             bool found_0 = false;
00315             for (uint j=0; j<cand_kfs.size(); j++)
00316             {
00317               if (cand_kfs[j] == cand_kf)
00318               {
00319                 found_0 = true;
00320                 break;
00321               }
00322             }
00323             if (!found_0)
00324               cand_kfs.push_back(cand_kf);
00325 
00326             // Search if this pair already exists
00327             bool found_1 = false;
00328             uint idx = 0;
00329             for (uint j=0; j<cluster_pairs.size(); j++)
00330             {
00331               if (cluster_pairs[j][0] == query_cluster && cluster_pairs[j][1] == cand_cluster)
00332               {
00333                 found_1 = true;
00334                 idx = j;
00335                 break;
00336               }
00337             }
00338 
00339             if (found_1)
00340               inliers_per_pair[idx]++;
00341             else
00342             {
00343               vector<int> pair;
00344               pair.push_back(query_cluster);
00345               pair.push_back(cand_cluster);
00346               cluster_pairs.push_back(pair);
00347               inliers_per_pair.push_back(1);
00348             }
00349           }
00350 
00351           // Add the corresponding edges
00352           vector< vector<int> > definitive_cluster_pairs;
00353           vector<int> definitive_inliers_per_pair;
00354           bool some_edge_added = false;
00355           for (uint i=0; i<inliers_per_pair.size(); i++)
00356           {
00357             if (inliers_per_pair[i] >= 5)
00358             {
00359               // Check if this frame exists
00360               bool lc_found = false;
00361               for (uint l=0; l<cluster_lc_found_.size(); l++)
00362               {
00363                 if ( (cluster_lc_found_[l].first == cluster_pairs[i][0] && cluster_lc_found_[l].second == cluster_pairs[i][1]) ||
00364                     (cluster_lc_found_[l].first == cluster_pairs[i][1] && cluster_lc_found_[l].second == cluster_pairs[i][0]) )
00365                 {
00366                   lc_found = true;
00367                   break;
00368                 }
00369               }
00370               if (lc_found) continue;
00371 
00372               tf::Transform candidate_cluster_pose = graph_->getVertexPose(cluster_pairs[i][1]);
00373               tf::Transform frame_cluster_pose_relative_to_camera = graph_->getVertexPoseRelativeToCamera(cluster_pairs[i][0]);
00374               tf::Transform edge_1 = candidate_cluster_pose.inverse() * estimated_transform * frame_cluster_pose_relative_to_camera;
00375 
00376               // Add this edge to the graph
00377               graph_->addEdge(cluster_pairs[i][1], cluster_pairs[i][0], edge_1, inliers_per_pair[i]);
00378               vector<int> pair;
00379               pair.push_back(cluster_pairs[i][0]);
00380               pair.push_back(cluster_pairs[i][1]);
00381               definitive_cluster_pairs.push_back(pair);
00382               definitive_inliers_per_pair.push_back(inliers_per_pair[i]);
00383               some_edge_added = true;
00384 
00385               // Add this edge to the cluster list of loop closings found
00386               cluster_lc_found_.push_back(make_pair(cluster_pairs[i][0], cluster_pairs[i][1]));
00387             }
00388           }
00389 
00390           if (some_edge_added)
00391           {
00392             num_loop_closures_++;
00393 
00394             // Update the graph with the new edges
00395             graph_->update();
00396 
00397             // Draw the loop closure to image
00398             drawLoopClosure(cand_kfs,
00399                             cand_matchings,
00400                             inliers,
00401                             definitive_inliers_per_pair,
00402                             definitive_cluster_pairs,
00403                             matched_query_kp_l,
00404                             matched_cand_kp_l);
00405             return true;
00406           }
00407         }
00408       }
00409     }
00410 
00411     return false;
00412   }
00413 
00414   void LoopClosing::getCandidates(int cluster_id, vector< pair<int,float> >& candidates)
00415   {
00416     // Init
00417     candidates.clear();
00418 
00419     // Check if enough neighbors
00420     if ((int)hash_table_.size() <= LC_DISCARD_WINDOW) return;
00421 
00422     // Create a list with the non-possible candidates (because they are already loop closings)
00423     vector<int> no_candidates;
00424     for (uint i=0; i<cluster_lc_found_.size(); i++)
00425     {
00426       if (cluster_lc_found_[i].first == cluster_id)
00427         no_candidates.push_back(cluster_lc_found_[i].second);
00428       if (cluster_lc_found_[i].second == cluster_id)
00429         no_candidates.push_back(cluster_lc_found_[i].first);
00430     }
00431 
00432     // Query hash
00433     vector<float> hash_q = hash_table_[cluster_id].second;
00434 
00435     // Loop over all the hashes stored
00436     vector< pair<int,float> > all_matchings;
00437     for (uint i=0; i<hash_table_.size(); i++)
00438     {
00439       // Discard window
00440       if (hash_table_[i].first > cluster_id-LC_DISCARD_WINDOW && hash_table_[i].first < cluster_id+LC_DISCARD_WINDOW) continue;
00441 
00442       // Do not compute the hash matching with itself
00443       if (hash_table_[i].first == cluster_id) continue;
00444 
00445       // Continue if candidate is in the no_candidates list
00446       if (find(no_candidates.begin(), no_candidates.end(), hash_table_[i].first) != no_candidates.end())
00447         continue;
00448 
00449       // Hash matching
00450       vector<float> hash_t = hash_table_[i].second;
00451       float m = hash_.match(hash_q, hash_t);
00452       all_matchings.push_back(make_pair(hash_table_[i].first, m));
00453     }
00454 
00455     // Sort the hash matchings
00456     sort(all_matchings.begin(), all_matchings.end(), Tools::sortByMatching);
00457 
00458     // Retrieve the best n matches
00459     uint max_size = 5;
00460     if (max_size > all_matchings.size()) max_size = all_matchings.size();
00461     for (uint i=0; i<max_size; i++)
00462       candidates.push_back(all_matchings[i]);
00463   }
00464 
00465   Cluster LoopClosing::readCluster(int id)
00466   {
00467     string file = execution_dir_+"/"+lexical_cast<string>(id)+".yml";
00468     Cluster cluster;
00469 
00470     // Sanity check
00471     if ( !fs::exists(file) ) return cluster;
00472 
00473     cv::FileStorage fs;
00474     fs.open(file, cv::FileStorage::READ);
00475     if (!fs.isOpened()) return cluster;
00476 
00477     int frame_id;
00478     vector<cv::KeyPoint> kp_l, kp_r;
00479     cv::Mat desc, pose, empty;
00480     vector<cv::Point3f> points;
00481     fs["frame_id"] >> frame_id;
00482     fs["desc"] >> desc;
00483     fs["points"] >> points;
00484     cv::FileNode kp_l_node = fs["kp_l"];
00485     cv::FileNode kp_r_node = fs["kp_r"];
00486     read(kp_l_node, kp_l);
00487     read(kp_r_node, kp_r);
00488     fs.release();
00489 
00490     // Set the properties of the cluster
00491     tf::Transform vertex_camera_pose = graph_->getVertexCameraPose(id, true);
00492     Cluster cluster_tmp(id, frame_id, vertex_camera_pose, kp_l, kp_r, desc, empty, points);
00493     cluster = cluster_tmp;
00494 
00495     return cluster;
00496   }
00497 
00498   void LoopClosing::drawLoopClosure(vector<int> cand_kfs,
00499                                     vector<int> cand_matchings,
00500                                     vector<int> inliers,
00501                                     vector<int> definitive_inliers_per_pair,
00502                                     vector< vector<int> > definitive_cluster_pairs,
00503                                     vector<cv::Point2f> matched_query_kp_l,
00504                                     vector<cv::Point2f> matched_cand_kp_l)
00505   {
00506     // Build image with loop closure matchings
00507     cv::Mat lc_image;
00508 
00509     // Read candidate keyframes
00510     vector<int> idx_img_candidate_kfs;
00511     cv::Mat img_candidate_kfs;
00512     int baseline = 0;
00513     for (uint i=0; i<cand_kfs.size(); i++)
00514     {
00515       string frame_id_str = Tools::convertTo5digits(cand_kfs[i]);
00516       string keyframe_file = WORKING_DIRECTORY + "keyframes/" + frame_id_str + ".jpg";
00517       cv::Mat kf = cv::imread(keyframe_file, CV_LOAD_IMAGE_COLOR);
00518 
00519       // Add the keyframe identifier
00520       stringstream s;
00521       s << " Keyframe " << cand_kfs[i];
00522       cv::Size text_size = cv::getTextSize(s.str(), cv::FONT_HERSHEY_PLAIN, 1, 1, &baseline);
00523       cv::Mat im_text = cv::Mat(kf.rows + text_size.height + 10, kf.cols, kf.type());
00524       kf.copyTo(im_text.rowRange(0, kf.rows).colRange(0, kf.cols));
00525       im_text.rowRange(kf.rows, im_text.rows).setTo(cv::Scalar(255,255,255));
00526       cv::putText(im_text, s.str(), cv::Point(5, im_text.rows - 5), cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(0,0,0), 1, 8);
00527 
00528       if (img_candidate_kfs.cols == 0)
00529         im_text.copyTo(img_candidate_kfs);
00530       else
00531         cv::hconcat(img_candidate_kfs, im_text, img_candidate_kfs);
00532 
00533       // Store the index
00534       idx_img_candidate_kfs.push_back(cand_kfs[i]);
00535     }
00536 
00537     // Read the current keyframe
00538     string frame_id_str = Tools::convertTo5digits(c_cluster_.getFrameId());
00539     string keyframe_file = WORKING_DIRECTORY + "keyframes/" + frame_id_str + ".jpg";
00540     cv::Mat current_kf_tmp = cv::imread(keyframe_file, CV_LOAD_IMAGE_COLOR);
00541 
00542     // Add the keyframe identifier
00543     stringstream s;
00544     s << " Keyframe " << c_cluster_.getFrameId();
00545     cv::Size text_size = cv::getTextSize(s.str(), cv::FONT_HERSHEY_PLAIN, 1, 1, &baseline);
00546     cv::Mat current_kf_text = cv::Mat(current_kf_tmp.rows + text_size.height + 10, current_kf_tmp.cols, current_kf_tmp.type());
00547     current_kf_tmp.copyTo(current_kf_text.rowRange(text_size.height + 10, current_kf_tmp.rows + text_size.height + 10).colRange(0, current_kf_tmp.cols));
00548     current_kf_text.rowRange(0, text_size.height + 10).setTo(cv::Scalar(255,255,255));
00549     cv::putText(current_kf_text, s.str(), cv::Point(5, 14), cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(0,0,0), 1, 8);
00550 
00551     // Compose current keyframe with candidate keyframes
00552     cv::Mat current_kf(img_candidate_kfs.rows, img_candidate_kfs.cols, img_candidate_kfs.type(), cv::Scalar(0, 0, 0));
00553     int x_offset = round(img_candidate_kfs.cols/2 - current_kf_text.cols/2);
00554     current_kf_text.copyTo( current_kf( cv::Rect(x_offset, 0, current_kf_text.cols, current_kf_text.rows) ) );
00555     cv::vconcat(current_kf, img_candidate_kfs, lc_image);
00556 
00557     // Build the vector of colors
00558     cv::RNG rng(12345);
00559     vector<cv::Scalar> colors;
00560     for (uint i=0; i<definitive_inliers_per_pair.size(); i++)
00561     {
00562       cv::Scalar color = cv::Scalar(rng.uniform(0,255), rng.uniform(0, 255), rng.uniform(0, 255));
00563       colors.push_back(color);
00564     }
00565 
00566     // Draw the matchings
00567     for (uint i=0; i<inliers.size(); i++)
00568     {
00569       // Extract the keypoint for the current keyframe
00570       cv::Point2f current_kp = matched_query_kp_l[inliers[i]];
00571       cv::Point2f candidate_kp = matched_cand_kp_l[inliers[i]];
00572 
00573       // Candidate cluster identifier
00574       int cand_cluster = cand_matchings[inliers[i]];
00575       int cand_keyframe = graph_->getVertexFrameId(cand_cluster);
00576 
00577       int position = -1;
00578       for (uint j=0; j<idx_img_candidate_kfs.size(); j++)
00579       {
00580         if (idx_img_candidate_kfs[j] == cand_keyframe)
00581         {
00582           position = j;
00583           break;
00584         }
00585       }
00586 
00587       // Draw
00588       if (position >= 0)
00589       {
00590         // Decide the color
00591         int color_idx = -1;
00592         for (uint n=0; n<definitive_inliers_per_pair.size(); n++)
00593         {
00594           if (definitive_cluster_pairs[n][1] == cand_cluster)
00595           {
00596             color_idx = n;
00597             break;
00598           }
00599         }
00600         if (color_idx >= 0)
00601         {
00602           cv::Point2f p_cur_kf = current_kp;
00603           p_cur_kf.x = x_offset + p_cur_kf.x;
00604 
00605           cv::Point2f p_cand_kf = candidate_kp;
00606           p_cand_kf.y = p_cand_kf.y + current_kf.rows;
00607           p_cand_kf.x = position*current_kf_text.cols + p_cand_kf.x;
00608 
00609           cv::circle(lc_image, p_cur_kf, 4, colors[color_idx], -1);
00610           cv::circle(lc_image, p_cand_kf, 4, colors[color_idx], -1);
00611           cv::line(lc_image, p_cur_kf, p_cand_kf, colors[color_idx], 2, 8, 0);
00612         }
00613       }
00614     }
00615 
00616     // Save
00617     string lc_file = loop_closures_dir_ + "/" + Tools::convertTo5digits(num_loop_closures_) + ".jpg";
00618     cv::imwrite( lc_file, lc_image );
00619 
00620     // Publish
00621     if (pub_lc_matchings_.getNumSubscribers() > 0)
00622     {
00623       cv_bridge::CvImage ros_image;
00624       ros_image.image = lc_image.clone();
00625       ros_image.header.stamp = ros::Time::now();
00626       ros_image.encoding = "bgr8";
00627       pub_lc_matchings_.publish(ros_image.toImageMsg());
00628     }
00629   }
00630 
00631   void LoopClosing::finalize()
00632   {
00633     // Remove the temporal directory
00634     if (fs::is_directory(execution_dir_))
00635       fs::remove_all(execution_dir_);
00636   }
00637 
00638 } //namespace slam


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