frame.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 
00003 #include "frame.h"
00004 #include "constants.h"
00005 #include "tools.h"
00006 
00007 using namespace tools;
00008 
00009 namespace slam
00010 {
00011 
00012   Frame::Frame() : pointcloud_(new PointCloudRGB) {}
00013 
00014   Frame::Frame(cv::Mat l_img,
00015                cv::Mat r_img,
00016                image_geometry::StereoCameraModel camera_model,
00017                double timestamp) : pointcloud_(new PointCloudRGB)
00018   {
00019     // Init
00020     id_ = -1;
00021     stamp_ = timestamp;
00022     num_inliers_with_prev_frame_ = LC_MIN_INLIERS;
00023 
00024     l_img.copyTo(l_img_);
00025     r_img.copyTo(r_img_);
00026 
00027     // Convert images to grayscale
00028     cv::Mat l_img_gray, r_img_gray;
00029     cv::cvtColor(l_img, l_img_gray, CV_RGB2GRAY);
00030     cv::cvtColor(r_img, r_img_gray, CV_RGB2GRAY);
00031 
00032     // Detect keypoints
00033     vector<cv::KeyPoint> l_kp, r_kp;
00034     cv::ORB orb(1500, 1.2, 8, 10, 0, 2, 0, 10);
00035     orb(l_img_gray, cv::noArray(), l_kp, cv::noArray(), false);
00036     orb(r_img_gray, cv::noArray(), r_kp, cv::noArray(), false);
00037 
00038     // Extract descriptors
00039     cv::Mat l_desc, r_desc;
00040     cv::Ptr<cv::DescriptorExtractor> cv_extractor;
00041     cv_extractor = cv::DescriptorExtractor::create("ORB");
00042     cv_extractor->compute(l_img_gray, l_kp, l_desc);
00043     cv_extractor->compute(r_img_gray, r_kp, r_desc);
00044 
00045     // Left/right matching
00046     vector<cv::DMatch> matches, matches_filtered;
00047     Tools::ratioMatching(l_desc, r_desc, 0.8, matches);
00048 
00049     // Filter matches by epipolar
00050     for (size_t i=0; i<matches.size(); ++i)
00051     {
00052       if (abs(l_kp[matches[i].queryIdx].pt.y - r_kp[matches[i].trainIdx].pt.y) < 1.5)
00053         matches_filtered.push_back(matches[i]);
00054     }
00055 
00056     // Compute 3D points
00057     l_kp_.clear();
00058     r_kp_.clear();
00059     camera_points_.clear();
00060     l_desc_.release();
00061     r_desc_.release();
00062     for (size_t i=0; i<matches_filtered.size(); ++i)
00063     {
00064       cv::Point3d world_point;
00065       int l_idx = matches_filtered[i].queryIdx;
00066       int r_idx = matches_filtered[i].trainIdx;
00067 
00068       cv::Point2d l_point = l_kp[l_idx].pt;
00069       cv::Point2d r_point = r_kp[r_idx].pt;
00070 
00071       double disparity = l_point.x - r_point.x;
00072       camera_model.projectDisparityTo3d(l_point, disparity, world_point);
00073 
00074       if ( isfinite(world_point.x) && isfinite(world_point.y) && isfinite(world_point.z) && world_point.z > 0)
00075       {
00076         // Save
00077         l_kp_.push_back(l_kp[l_idx]);
00078         r_kp_.push_back(r_kp[r_idx]);
00079         l_desc_.push_back(l_desc.row(l_idx));
00080         r_desc_.push_back(r_desc.row(r_idx));
00081         camera_points_.push_back(world_point);
00082       }
00083     }
00084   }
00085 
00086   cv::Mat Frame::computeSift()
00087   {
00088     cv::Mat sift;
00089     if (l_img_.cols == 0)
00090       return sift;
00091 
00092     cv::initModule_nonfree();
00093     cv::Ptr<cv::DescriptorExtractor> cv_extractor;
00094     cv_extractor = cv::DescriptorExtractor::create("SIFT");
00095     cv_extractor->compute(l_img_, l_kp_, sift);
00096 
00097     return sift;
00098   }
00099 
00100   // FROM: http://codereview.stackexchange.com/questions/23966/density-based-clustering-of-image-keypoints
00101   void Frame::regionClustering()
00102   {
00103     clusters_.clear();
00104     vector< vector<int> > clusters;
00105     const float eps = 50.0;
00106     const int min_pts = 20;
00107     vector<bool> clustered;
00108     vector<int> noise;
00109     vector<bool> visited;
00110     vector<int> neighbor_pts;
00111     vector<int> neighbor_pts_;
00112     int c;
00113 
00114     uint no_keys = l_kp_.size();
00115 
00116     //init clustered and visited
00117     for(uint k=0; k<no_keys; k++)
00118     {
00119       clustered.push_back(false);
00120       visited.push_back(false);
00121     }
00122 
00123     c = -1;
00124 
00125     //for each unvisited point P in dataset keypoints
00126     for(uint i=0; i<no_keys; i++)
00127     {
00128       if(!visited[i])
00129       {
00130         // Mark P as visited
00131         visited[i] = true;
00132         neighbor_pts = regionQuery(&l_kp_, &l_kp_.at(i), eps);
00133         if(neighbor_pts.size() < min_pts)
00134         {
00135           // Mark P as Noise
00136           noise.push_back(i);
00137           clustered[i] = true;
00138         }
00139         else
00140         {
00141           clusters.push_back(vector<int>());
00142           c++;
00143 
00144           // expand cluster
00145           // add P to cluster c
00146           clusters[c].push_back(i);
00147           clustered[i] = true;
00148 
00149           // for each point P' in neighbor_pts
00150           for(uint j=0; j<neighbor_pts.size(); j++)
00151           {
00152             // if P' is not visited
00153             if(!visited[neighbor_pts[j]])
00154             {
00155               // Mark P' as visited
00156               visited[neighbor_pts[j]] = true;
00157               neighbor_pts_ = regionQuery(&l_kp_, &l_kp_.at(neighbor_pts[j]), eps);
00158               if(neighbor_pts_.size() >= min_pts)
00159               {
00160                 neighbor_pts.insert(neighbor_pts.end(), neighbor_pts_.begin(), neighbor_pts_.end());
00161               }
00162             }
00163             // if P' is not yet a member of any cluster
00164             // add P' to cluster c
00165             if(!clustered[neighbor_pts[j]])
00166             {
00167               clusters[c].push_back(neighbor_pts[j]);
00168               clustered[neighbor_pts[j]] = true;
00169             }
00170           }
00171         }
00172       }
00173     }
00174 
00175     // Discard small clusters
00176     for (uint i=0; i<clusters.size(); i++)
00177     {
00178       if (clusters[i].size() >= min_pts)
00179         clusters_.push_back(clusters[i]);
00180       else
00181       {
00182         for (uint j=0; j<clusters[i].size(); j++)
00183           noise.push_back(clusters[i][j]);
00184       }
00185     }
00186 
00187     // Refine points treated as noise
00188     bool iterate = true;
00189     while (iterate && noise.size() > 0)
00190     {
00191       uint size_a = noise.size();
00192       vector<int> noise_tmp;
00193       for (uint n=0; n<noise.size(); n++)
00194       {
00195         int idx = -1;
00196         bool found = false;
00197         cv::KeyPoint p_n = l_kp_.at(noise[n]);
00198         for (uint i=0; i<clusters_.size(); i++)
00199         {
00200           for (uint j=0; j<clusters_[i].size(); j++)
00201           {
00202             cv::KeyPoint p_c = l_kp_.at(clusters_[i][j]);
00203             float dist = sqrt(pow((p_c.pt.x - p_n.pt.x),2)+pow((p_c.pt.y - p_n.pt.y),2));
00204             if(dist <= eps && dist != 0.0)
00205             {
00206               idx = i;
00207               found = true;
00208               break;
00209             }
00210           }
00211           if (found)
00212             break;
00213         }
00214 
00215         if (found && idx >= 0)
00216           clusters_[idx].push_back(noise[n]);
00217         else
00218           noise_tmp.push_back(noise[n]);
00219       }
00220 
00221       if (noise_tmp.size() == 0 || noise_tmp.size() == size_a)
00222         iterate = false;
00223 
00224       noise = noise_tmp;
00225     }
00226 
00227     // If 1 cluster, add all keypoints
00228     if (clusters_.size() == 1)
00229     {
00230       vector<int> cluster_tmp;
00231       for (uint i=0; i<l_kp_.size(); i++)
00232         cluster_tmp.push_back(int(i));
00233       clusters_.clear();
00234       clusters_.push_back(cluster_tmp);
00235     }
00236 
00237     // Compute the clusters centroids
00238     for (uint i=0; i<clusters_.size(); i++)
00239     {
00240       PointCloudXYZ::Ptr cluster_points(new PointCloudXYZ);
00241       for (uint j=0; j<clusters_[i].size(); j++)
00242       {
00243         int idx = clusters_[i][j];
00244         cv::Point3f p_cv = camera_points_[idx];
00245         PointXYZ p(p_cv.x, p_cv.y, p_cv.z);
00246         cluster_points->push_back(p);
00247       }
00248 
00249       Eigen::Vector4f centroid;
00250       compute3DCentroid(*cluster_points, centroid);
00251       cluster_centroids_.push_back(centroid);
00252     }
00253   }
00254 
00255   vector<int> Frame::regionQuery(vector<cv::KeyPoint> *keypoints, cv::KeyPoint *keypoint, float eps)
00256   {
00257     float dist;
00258     vector<int> ret_keys;
00259     for(uint i=0; i<keypoints->size(); i++)
00260     {
00261       dist = sqrt(pow((keypoint->pt.x - keypoints->at(i).pt.x),2)+pow((keypoint->pt.y - keypoints->at(i).pt.y),2));
00262       if(dist <= eps && dist != 0.0)
00263       {
00264         ret_keys.push_back(i);
00265       }
00266     }
00267     return ret_keys;
00268   }
00269 
00270 } //namespace slam


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