ransac_simple.h
Go to the documentation of this file.
00001 #ifndef PCL_SAMPLE_CONSENSUS_RANSAC_SIMPLE_H_
00002 #define PCL_SAMPLE_CONSENSUS_RANSAC_SIMPLE_H_
00003 
00004 #include <pcl17/sample_consensus/sac.h>
00005 #include <pcl17/sample_consensus/sac_model.h>
00006 #include <pcl17/filters/passthrough.h>
00007 #include <pcl17/octree/octree_search.h>
00008 #include <pcl17/common/transforms.h>
00009 #include <pcl17/visualization/pcl_visualizer.h>
00010 
00011 namespace pcl17
00012 {
00013 template<typename PointT>
00014   class RandomSampleConsensusSimple
00015   {
00016   public:
00017     RandomSampleConsensusSimple(float octree_res) :
00018       scene_octree_(octree_res)
00019     {
00020       max_iterations_ = 10000;
00021       eps_ = 0.01;
00022       weight_ = 2;
00023     }
00024 
00025     void setMaxIterations(int max_iterations)
00026     {
00027       max_iterations_ = max_iterations;
00028     }
00029 
00030     int getMaxIterations()
00031     {
00032       return max_iterations_;
00033     }
00034 
00035     void setScene(typename pcl17::PointCloud<PointT>::Ptr & scene)
00036     {
00037       scene_ = scene;
00038       scene_octree_.setInputCloud(scene_);
00039       scene_octree_.addPointsFromInputCloud();
00040       std::cerr << "Tree depth " << scene_octree_.getTreeDepth() << " Scene size " << scene_->points.size()
00041           << std::endl;
00042     }
00043 
00044     void setSceneSegment(boost::shared_ptr<std::vector<int> > idx)
00045     {
00046       scene_segment_idx_ = idx;
00047     }
00048 
00049     void setModel(typename pcl17::PointCloud<PointT>::Ptr & model)
00050     {
00051       model_ = model;
00052     }
00053 
00054     void computeModel()
00055     {
00056 
00057       best_score_ = 0;
00058 
00059       int max_iter_all = max_iterations_ * 10;
00060       int iter_all = 0;
00061 
00062       for (int i = 0; i < max_iterations_; i++)
00063       {
00064 
00065         int sample = scene_segment_idx_->at(rand() % scene_segment_idx_->size());
00066 
00067         //        while (!isSampleGood(sample) && iter_all < max_iter_all)
00068         //        {
00069         //          iter_all++;
00070         //          sample = scene_segment_idx_->at(rand() % scene_segment_idx_->size());
00071         //        }
00072         //
00073         //        if(iter_all >= max_iter_all) continue;
00074 
00075         Eigen::VectorXf model_coefficients;
00076         while (!computeModelCoefficients(sample, model_coefficients) && iter_all < max_iter_all)
00077         {
00078           iter_all++;
00079         }
00080 
00081         if (iter_all >= max_iter_all)
00082           continue;
00083 
00084         float score = countScore(model_coefficients);
00085 
00086         if (score > best_score_)
00087         {
00088           best_model_coefficients_ = model_coefficients;
00089           best_score_ = score;
00090         }
00091 
00092       }
00093     }
00094 
00095     bool computeModelCoefficients(int sample, Eigen::VectorXf & model_coefficients)
00096     {
00097 
00098       PointT scene_point = scene_->points[sample];
00099 
00100       // Select points with the same height
00101       std::vector<int> idx;
00102 
00103       pcl17::PassThrough<PointT> pass;
00104       pass.setInputCloud(model_);
00105       pass.setFilterFieldName("z");
00106       pass.setFilterLimits(scene_point.z - eps_, scene_point.z + eps_);
00107       pass.filter(idx);
00108 
00109       if (idx.size() == 0)
00110         return false;
00111 
00112       int rand_idx = rand() % (int)idx.size();
00113 
00114       PointT model_point = model_->points[idx[rand_idx]];
00115 
00116       model_coefficients.resize(3);
00117 
00118       model_coefficients[0] = model_point.x - scene_point.x;
00119       model_coefficients[1] = model_point.y - scene_point.y;
00120 
00121       if (scene_point.z < 0.8)
00122       {
00123 
00124         model_coefficients[2] = atan2(model_point.normal_y, model_point.normal_x) - atan2(scene_point.normal_y,
00125                                                                                           scene_point.normal_x);
00126       } else {
00127         model_coefficients[2] = ((double) rand())/RAND_MAX * M_2_PI;
00128       }
00129       model_coefficients *= -1;
00130 
00131       return true;
00132 
00133     }
00134 
00135     float countScore(const Eigen::VectorXf & model_coefficients)
00136     {
00137       pcl17::PointCloud<PointT> transformed_model;
00138 
00139       Eigen::Affine3f transform;
00140       transform.setIdentity();
00141       transform.translate(Eigen::Vector3f(model_coefficients[0], model_coefficients[1], 0));
00142       transform.rotate(Eigen::AngleAxisf(model_coefficients[2], Eigen::Vector3f(0, 0, 1)));
00143 
00144       pcl17::transformPointCloudWithNormals(*model_, transformed_model, transform);
00145 
00146       float score = generateVisibilityScore(transformed_model);
00147 
00148       //std::cerr << "Score " << score << std::endl;
00149 
00150       return score;
00151 
00152     }
00153 
00154     float generateVisibilityScore(const pcl17::PointCloud<PointT> & cloud)
00155     {
00156 
00157       //      pcl17::visualization::PCLVisualizer viz;
00158       //
00159       //      viz.addPointCloud<PointT>(scene_);
00160       //
00161       //      typename pcl17::PointCloud<PointT>::Ptr cloud_ptr = cloud.makeShared();
00162       //
00163       //      pcl17::visualization::PointCloudColorHandlerCustom<PointT> single_color(cloud_ptr, 255, 0, 0);
00164       //
00165       //      viz.addPointCloud<PointT>(cloud_ptr, single_color, "cloud1");
00166       //
00167       //      viz.spin();
00168 
00169       int free = 0, occupied = 0, occluded = 0;
00170       for (size_t j = 0; j < cloud.points.size(); j++)
00171       {
00172         PointT point = cloud.points[j];
00173 
00174         if (scene_octree_.isVoxelOccupiedAtPoint(point))
00175         {
00176           occupied++;
00177 
00178           continue;
00179         }
00180 
00181         Eigen::Vector3f sensor_orig = scene_->sensor_origin_.head(3);
00182         //std::cerr << "Sensor origin\n" << sensor_orig << std::endl;
00183         Eigen::Vector3f look_at = point.getVector3fMap() - sensor_orig;
00184 
00185         std::vector<int> indices;
00186         scene_octree_.getIntersectedVoxelIndices(sensor_orig, look_at, indices);
00187 
00188         bool is_occluded = false;
00189         if (indices.size() > 0)
00190         {
00191           for (size_t k = 0; k < indices.size(); k++)
00192           {
00193             Eigen::Vector3f ray = scene_->points[indices[k]].getVector3fMap() - sensor_orig;
00194 
00195             if (ray.norm() < look_at.norm())
00196             {
00197               is_occluded = true;
00198             }
00199 
00200           }
00201         }
00202 
00203         if (is_occluded)
00204         {
00205           occluded++;
00206           continue;
00207         }
00208 
00209         free++;
00210 
00211       }
00212 
00213       //std::cerr << "Score " << occupied << " " << occluded << " " << free << std::endl;
00214       //return (weight_ * occupied + occluded) / (weight_ * occupied + occluded + free);
00215       return (weight_ * occupied + occluded) / (weight_ * occupied + occluded + weight_*free);
00216     }
00217 
00218     bool isSampleGood(int sample) const
00219     {
00220       return scene_->points[sample].normal_z < 0.8;
00221     }
00222 
00223     Eigen::VectorXf getBestModelCoeficients()
00224     {
00225       return best_model_coefficients_;
00226     }
00227 
00228     float getBestScore()
00229     {
00230       return best_score_;
00231     }
00232 
00233     void setWeight(float weight)
00234     {
00235       weight_ = weight;
00236     }
00237 
00238     typename pcl17::PointCloud<PointT>::Ptr getModel(){
00239       return model_;
00240     }
00241 
00242     pcl17::octree::OctreePointCloudSearch<PointT> scene_octree_;
00243     typename pcl17::PointCloud<PointT>::Ptr scene_;
00244     boost::shared_ptr<std::vector<int> > scene_segment_idx_;
00245     typename pcl17::PointCloud<PointT>::Ptr model_;
00246 
00247     Eigen::VectorXf best_model_coefficients_;
00248     float best_score_;
00249     float weight_;
00250 
00251     int max_iterations_;
00252     float eps_;
00253 
00254   };
00255 }
00256 
00257 #endif  //#ifndef PCL_SAMPLE_CONSENSUS_RANSAC_H_
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


furniture_classification
Author(s): Vladyslav Usenko
autogenerated on Sun Oct 6 2013 12:12:33