sac_3dof.h
Go to the documentation of this file.
00001 #ifndef SAC_3DOF_H_
00002 #define SAC_3DOF_H_
00003 
00004 #include <pcl17/sample_consensus/sac_model.h>
00005 #include <pcl17/sample_consensus/model_types.h>
00006 #include <pcl17/filters/passthrough.h>
00007 #include <pcl17/common/transforms.h>
00008 #include <pcl17/search/kdtree.h>
00009 #include <pcl17/io/pcd_io.h>
00010 #include <pcl17/octree/octree_search.h>
00011 
00012 namespace pcl17
00013 {
00014 
00015 template<typename PointT>
00016   class SampleConsensusModel3DOF : public SampleConsensusModel<PointT>
00017   {
00018     using SampleConsensusModel<PointT>::input_;
00019     using SampleConsensusModel<PointT>::indices_;
00020     using SampleConsensusModel<PointT>::samples_radius_search_;
00021 
00022   public:
00023     typedef typename SampleConsensusModel<PointT>::PointCloud PointCloud;
00024     typedef typename SampleConsensusModel<PointT>::PointCloudPtr PointCloudPtr;
00025     typedef typename SampleConsensusModel<PointT>::PointCloudConstPtr PointCloudConstPtr;
00026 
00027     typedef boost::shared_ptr<SampleConsensusModel3DOF> Ptr;
00028     typedef boost::shared_ptr<const SampleConsensusModel3DOF> ConstPtr;
00029 
00030     SampleConsensusModel3DOF(const PointCloudConstPtr & cloud, float octree_res) :
00031       SampleConsensusModel<PointT> (cloud), octree(octree_res)
00032     {
00033       setInputCloud(cloud);
00034       octree.setInputCloud(cloud);
00035       octree.addPointsFromInputCloud();
00036 
00037       std::cerr << "Sensor orig" << cloud->sensor_origin_ << std::endl;
00038 
00039     }
00040 
00041     PointCloudConstPtr getTarget() const
00042     {
00043       return target;
00044     }
00045 
00046     void setTarget(PointCloudConstPtr target)
00047     {
00048       this->target = target;
00049       this->target_tree.setInputCloud(this->target);
00050     }
00051 
00052     virtual bool computeModelCoefficients(const std::vector<int> & samples, Eigen::VectorXf & model_coefficients)
00053     {
00054 
00055       PointT input_point = input_->points[samples[0]];
00056       //std::cerr << "IP " << input_point << std::endl;
00057 
00058       // Select points with the same height
00059       std::vector<int> idx;
00060 
00061       pcl17::PassThrough<PointT> pass;
00062       pass.setInputCloud(target);
00063       pass.setIndices(target_idx);
00064       pass.setFilterFieldName("z");
00065       pass.setFilterLimits(input_point.z - eps, input_point.z + eps);
00066       pass.filter(idx);
00067 
00068       if (idx.size() == 0)
00069         return false;
00070 
00071       int rand_idx = rand() % (int)idx.size();
00072 
00073       PointT target_point = target->points[idx[rand_idx]];
00074       //std::cerr << "TP " << target_point << std::endl;
00075 
00076       model_coefficients.resize(3);
00077 
00078       model_coefficients[0] = target_point.x - input_point.x;
00079       model_coefficients[1] = target_point.y - input_point.y;
00080       model_coefficients[2] = atan2(target_point.normal_y, target_point.normal_x) - atan2(input_point.normal_y,
00081                                                                                           input_point.normal_x);
00082 
00083       model_coefficients *= -1;
00084 
00085       //      PointCloud transformed_input;
00086       //
00087       //      Eigen::Affine3f transform;
00088       //      transform.setIdentity();
00089       //      transform.translate(Eigen::Vector3f(model_coefficients[0], model_coefficients[1], 0));
00090       //      transform.rotate(Eigen::AngleAxisf(model_coefficients[2], Eigen::Vector3f(0, 0, 1)));
00091       //
00092       //      //pcl17::io::savePCDFileASCII("tmp_inp.pcd", *input_);
00093       //      //pcl17::io::savePCDFileASCII("tmp_tgt.pcd", *target);
00094       //      ///std::cerr << "Transform " << transform.matrix() << std::endl;
00095       //      pcl17::transformPointCloudWithNormals(*input_, transformed_input, transform);
00096       //      //pcl17::io::savePCDFileASCII("tmp_res.pcd", transformed_input);
00097       //      PointT input_point_transformed = transformed_input.points[samples[0]];
00098       //
00099       //      //std::cerr << "IPT " << input_point_transformed << std::endl;
00100       //      //std::cerr << "MC " << model_coefficients << std::endl;
00101 
00102       return true;
00103 
00104     }
00105 
00106     virtual void selectWithinDistance(const Eigen::VectorXf & model_coefficients, const double threshold, std::vector<
00107         int> & inliers)
00108     {
00109 
00110       // return vector of zeros of size num_inliers
00111       int num_inliers = countWithinDistance(model_coefficients, threshold);
00112       inliers.clear();
00113       inliers.push_back(num_inliers);
00114 
00115       std::cerr << "Returned weight " << inliers[0] << std::endl;
00116     }
00117 
00118     virtual int countWithinDistance(const Eigen::VectorXf & model_coefficients, const double threshold)
00119     {
00120       PointCloud transformed_input;
00121 
00122       Eigen::Affine3f transform;
00123       transform.setIdentity();
00124       transform.translate(Eigen::Vector3f(model_coefficients[0], model_coefficients[1], 0));
00125       transform.rotate(Eigen::AngleAxisf(model_coefficients[2], Eigen::Vector3f(0, 0, 1)));
00126 
00127       pcl17::transformPointCloudWithNormals(*target, transformed_input, transform);
00128 
00129       std::vector<int> idx;
00130       std::vector<float> dist;
00131 
00132       int num_inliers = ((float)INT_MAX) * generateVisibilityScore(transformed_input);
00133 
00134       std::cerr << "Number of inliers " << num_inliers << std::endl;
00135 
00136       return num_inliers;
00137 
00138     }
00139 
00140     virtual void optimizeModelCoefficients(const std::vector<int> & inliers,
00141                                            const Eigen::VectorXf & model_coefficients,
00142                                            Eigen::VectorXf & optimized_coefficients)
00143     {
00144       optimized_coefficients = model_coefficients;
00145     }
00146 
00147     virtual void getDistancesToModel(const Eigen::VectorXf & model_coefficients, std::vector<double> & distances)
00148     {
00149 
00150     }
00151 
00152     virtual void projectPoints(const std::vector<int> & inliers, const Eigen::VectorXf & model_coefficients,
00153                                PointCloud & projected_points, bool copy_data_fields = true)
00154     {
00155     }
00156 
00157     virtual bool doSamplesVerifyModel(const std::set<int> & indices, const Eigen::VectorXf & model_coefficients,
00158                                       const double threshold)
00159     {
00160       return true;
00161     }
00162 
00163     virtual SacModel getModelType() const
00164     {
00165       return pcl17::SACMODEL_SPHERE;
00166     }
00167 
00168     inline virtual bool isModelValid(const Eigen::VectorXf &model_coefficients)
00169     {
00170       return true;
00171     }
00172     virtual bool isSampleGood(const std::vector<int> &samples) const
00173     {
00174       return input_->points[samples[0]].normal_z < 0.8;
00175     }
00176 
00177     inline unsigned int getSampleSize() const
00178     {
00179       return 1;
00180     }
00181 
00182     void setTargetIndices(boost::shared_ptr<std::vector<int> > & idx)
00183     {
00184       target_idx = idx;
00185     }
00186 
00187   protected:
00188 
00189     float generateVisibilityScore(const PointCloud & cloud)
00190     {
00191 
00192       int free = 0, occupied = 0, occluded = 0;
00193       for (size_t j = 0; j < cloud.points.size(); j++)
00194       {
00195         PointT point = cloud.points[j];
00196 
00197         if (octree.isVoxelOccupiedAtPoint(point))
00198         {
00199           occupied++;
00200 
00201           continue;
00202         }
00203 
00204         Eigen::Vector3f sensor_orig = input_->sensor_origin_.head(3);
00205         Eigen::Vector3f look_at = point.getVector3fMap() - sensor_orig;
00206 
00207         std::vector<int> indices;
00208         octree.getIntersectedVoxelIndices(sensor_orig, look_at, indices);
00209 
00210         bool is_occluded = false;
00211         if (indices.size() > 0)
00212         {
00213           for (size_t k = 0; k < indices.size(); k++)
00214           {
00215             Eigen::Vector3f ray = input_->points[indices[k]].getVector3fMap() - sensor_orig;
00216 
00217             if (ray.norm() < look_at.norm())
00218             {
00219               is_occluded = true;
00220             }
00221 
00222           }
00223         }
00224 
00225         if (is_occluded)
00226         {
00227           occluded++;
00228           continue;
00229         }
00230 
00231         free++;
00232 
00233       }
00234 
00235       return ((float)2 * occupied + occluded) / (2 * occupied + occluded + free);
00236       //std::cerr << "Score " << occupied << " " << occluded << " " << free << " " << score[i] << std::endl;
00237 
00238 
00239     }
00240 
00241     pcl17::octree::OctreePointCloudSearch<PointT> octree;
00242     PointCloudConstPtr target;
00243     boost::shared_ptr<std::vector<int> > target_idx;
00244     pcl17::search::KdTree<PointT> target_tree;
00245 
00246     float eps;
00247 
00248   };
00249 }
00250 
00251 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


furniture_classification
Author(s): Vladyslav Usenko
autogenerated on Thu May 23 2013 18:32:29