region_adjacency_graph.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2014, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 #include <jsk_pcl_ros/region_adjacency_graph.h>
00037 #include <jsk_topic_tools/log_utils.h>
00038 
00039 namespace jsk_pcl_ros
00040 {   
00041     RegionAdjacencyGraph::RegionAdjacencyGraph()
00042     {
00043        
00044     }
00045    
00046     void RegionAdjacencyGraph::generateRAG(
00047        const std::vector<pcl::PointCloud<PointT>::Ptr> &cloud_clusters,
00048        const std::vector<pcl::PointCloud<pcl::Normal>::Ptr>  &normal_clusters,
00049        const pcl::PointCloud<pcl::PointXYZ>::Ptr centroids,
00050        std::vector<std::vector<int> > &neigbor_indices,
00051        const int edge_weight_criteria)
00052     {
00053        if (cloud_clusters.empty() || normal_clusters.empty() ||
00054            centroids->empty() || neigbor_indices.empty()) {
00055           ROS_ERROR("ERROR: Cannot Generate RAG of empty data...");
00056           return;
00057        }
00058        const int comparision_points_size = 100;
00059        if (cloud_clusters.size() == neigbor_indices.size()) {
00060           std::vector<VertexDescriptor> vertex_descriptor;
00061           for (int j = 0; j < centroids->size(); j++) {
00062              VertexDescriptor v_des = boost::add_vertex(
00063                 VertexProperty(j, centroids->points[j], -1), this->graph_);
00064              vertex_descriptor.push_back(v_des);
00065           }
00066           for (int j = 0; j < neigbor_indices.size(); j++) {
00067              VertexDescriptor r_vd = vertex_descriptor[j];
00068              std::vector<Eigen::Vector3f> center_point;
00069              std::vector<Eigen::Vector3f> center_normal;
00070              cv::Mat r_histogram;
00071              if (edge_weight_criteria == RAG_EDGE_WEIGHT_CONVEX_CRITERIA) {
00072                 this->sampleRandomPointsFromCloudCluster(
00073                    cloud_clusters[j],
00074                    normal_clusters[j],
00075                    center_point,
00076                    center_normal,
00077                    comparision_points_size);
00078              } else if (edge_weight_criteria == RAG_EDGE_WEIGHT_DISTANCE) {
00079                 if (cloud_clusters[j]->size() > sizeof(char) &&
00080                     normal_clusters[j]->size() > sizeof(char)) {
00081                    this->computeCloudClusterRPYHistogram(
00082                       cloud_clusters[j],
00083                       normal_clusters[j],
00084                       r_histogram);
00085                 }
00086              } else {
00087                 ROS_ERROR("Incorrect Measurement type");
00088                 return;
00089              }
00090              for (int i = 0; i < neigbor_indices[j].size(); i++) {
00091                 int n_index = neigbor_indices[j][i];
00092                 VertexDescriptor vd = vertex_descriptor[n_index];
00093                 float distance = 0.0f;
00094                 if (edge_weight_criteria == RAG_EDGE_WEIGHT_CONVEX_CRITERIA) {
00095                    std::vector<Eigen::Vector3f> n1_point;
00096                    std::vector<Eigen::Vector3f> n1_normal;
00097                    this->sampleRandomPointsFromCloudCluster(
00098                       cloud_clusters[n_index],
00099                       normal_clusters[n_index],
00100                       n1_point,
00101                       n1_normal,
00102                       comparision_points_size);
00103                    // Common Neigbour
00104                    int commonIndex = this->getCommonNeigbour(
00105                       neigbor_indices[j],
00106                       neigbor_indices[n_index]);
00107                    if (commonIndex == -1) {
00108                       // distance = this->getCloudClusterWeightFunction<float>(
00109                       //    center_point, n1_point, center_normal, n1_normal);
00110                    } else {
00111                       std::vector<Eigen::Vector3f> n2_point;
00112                       std::vector<Eigen::Vector3f> n2_normal;
00113                       this->sampleRandomPointsFromCloudCluster(
00114                          cloud_clusters[commonIndex],
00115                          normal_clusters[commonIndex],
00116                          n2_point,
00117                          n2_normal,
00118                          comparision_points_size);
00119                       std::vector<std::vector<Eigen::Vector3f> > _points;
00120                       std::vector<std::vector<Eigen::Vector3f> > _normals;
00121                       _points.push_back(center_point);
00122                       _points.push_back(n1_point);
00123                       // _points.push_back(n2_point);
00124                       _normals.push_back(center_normal);
00125                       _normals.push_back(n1_normal);
00126                       // _normals.push_back(n2_normal);
00127                       distance = this->getCloudClusterWeightFunction<float>(
00128                          _points, _normals);
00129                    }
00130                 } else if (edge_weight_criteria == RAG_EDGE_WEIGHT_DISTANCE) {
00131                    if (cloud_clusters[j]->size() > sizeof(char) &&
00132                        cloud_clusters[n_index]->size() > sizeof(char)) {
00133                       cv::Mat n_histogram;
00134                       this->computeCloudClusterRPYHistogram(
00135                          cloud_clusters[n_index],
00136                          normal_clusters[n_index],
00137                          n_histogram);
00138                       distance = static_cast<float>(
00139                          cv::compareHist(
00140                             r_histogram, n_histogram, CV_COMP_CORREL));
00141                    } else {
00142                       distance = 0.0f;
00143                    }
00144                 } else {
00145                    distance = 0.0f;
00146                 }
00147                 if (r_vd != vd) {
00148                    bool found = false;
00149                    EdgeDescriptor e_descriptor;
00150                    boost::tie(e_descriptor, found) = boost::edge(r_vd, vd, this->graph_);
00151                    if (!found) {
00152                       boost::add_edge(
00153                          r_vd, vd, EdgeProperty(distance), this->graph_);
00154                    }
00155                 }
00156              }
00157           }
00158        } else {
00159           ROS_WARN("Elements not same size..");
00160        }
00161     }
00162 
00163     template<typename T>
00164     T RegionAdjacencyGraph::getCloudClusterWeightFunction(
00165        const std::vector<std::vector<Eigen::Vector3f> > &_points,
00166        const std::vector<std::vector<Eigen::Vector3f> > &_normal)
00167     {
00168 #define ANGLE_THRESHOLD (10)
00169        if (_points.size() == 2 && _points.size() == _normal.size()) {
00170           T weights_ = -1.0f;
00171           int concave_ = 0;
00172           int convex_ = 0;
00173           for (int i = 0; i < _points[0].size(); i++) {
00174           T convexC_ij = this->convexityCriterion<T>(
00175              _points[0][i], _points[1][i], _normal[0][i], _normal[1][i]);
00176           float angle_ = getVectorAngle(_normal[0][i], _normal[1][i]);
00177           if (convexC_ij < 0.0f && angle_ < ANGLE_THRESHOLD) {
00178              convexC_ij = abs(convexC_ij);
00179           }
00180           if (convexC_ij > 0.0) {
00181              convex_++;
00182           }
00183           if (convexC_ij <= 0.0 || std::isnan(convexC_ij)) {
00184              concave_++;
00185           }
00186           /*
00187           if (convexC_ij > weights_) {
00188              weights_ = convexC_ij;
00189              }*/
00190           }
00191           if (concave_ < convex_ + 20) {
00192              weights_ = 1.0f;
00193           }
00194           return weights_;
00195        } else if (_points.size() == 3) {
00196           T weights_ = FLT_MIN;
00197           for (int i = 0; i < _points[0].size(); i++) {
00198              T convexC_ij = this->convexityCriterion<T>(
00199                 _points[0][i], _points[1][i], _normal[0][i], _normal[1][i]);
00200              T convexC_ic = this->convexityCriterion<T>(
00201                 _points[0][i], _points[2][i], _normal[0][i], _normal[2][i]);
00202              T convexC_jc = this->convexityCriterion<T>(
00203                 _points[1][i], _points[2][i], _normal[1][i], _normal[2][i]);
00204           // float angle_ = getVectorAngle(_normal[0][i], _normal[1][i]);
00205           // if (angle_ > ANGLE_THRESHOLD && convexC_ij <= 0) {
00206           //    convexC_ij =/ -1;
00207           // }
00208              weights_ = std::max(convexC_ij,
00209                                  std::max(convexC_ic, convexC_jc));
00210           }
00211           return weights_;
00212        }
00213     }
00214 
00215     float RegionAdjacencyGraph::getVectorAngle(
00216        const Eigen::Vector3f &vector1,
00217        const Eigen::Vector3f &vector2,
00218        bool indegree)
00219     {
00220        float angle_ = acos(vector1.dot(vector2));
00221        if (indegree) {
00222           return angle_ * 180/M_PI;
00223        } else {
00224           return angle_;
00225        }
00226     }
00227 
00228     template<typename T>
00229     T RegionAdjacencyGraph::convexityCriterion(
00230        const Eigen::Vector3f &center_point,
00231        const Eigen::Vector3f &n1_point,
00232        const Eigen::Vector3f &center_normal,
00233        const Eigen::Vector3f &neigbour_normal)
00234     {
00235        Eigen::Vector3f difference_ = center_point - n1_point;
00236        difference_ /= difference_.norm();
00237        T convexityc = static_cast<T>(center_normal.dot(difference_) -
00238                                      neigbour_normal.dot(difference_));
00239        return convexityc;
00240     }
00241 
00242     void RegionAdjacencyGraph::sampleRandomPointsFromCloudCluster(
00243        pcl::PointCloud<PointT>::Ptr cloud,
00244        pcl::PointCloud<pcl::Normal>::Ptr normal,
00245        std::vector<Eigen::Vector3f> &point_vector,
00246        std::vector<Eigen::Vector3f> &normal_vector,
00247        int gen_sz)
00248     {
00249        for (int i = 0; i < std::max(gen_sz, (int)cloud->size()); i++) {
00250           int _idx = rand() % cloud->size();
00251           Eigen::Vector3f cv = cloud->points[_idx].getVector3fMap();
00252           Eigen::Vector3f nv = Eigen::Vector3f(
00253              normal->points[_idx].normal_x,
00254              normal->points[_idx].normal_y,
00255              normal->points[_idx].normal_z);
00256           point_vector.push_back(cv);
00257           normal_vector.push_back(nv);
00258        }
00259     }
00260 
00261     void RegionAdjacencyGraph::splitMergeRAG(
00262        const int _threshold)
00263     {
00264        if (num_vertices(this->graph_) == 0) {
00265           ROS_ERROR("ERROR: Cannot Merge Empty RAG ...");
00266           return;
00267        }
00268        IndexMap index_map = get(boost::vertex_index, this->graph_);
00269        EdgePropertyAccess edge_weights = get(boost::edge_weight, this->graph_);
00270        VertexIterator i, end;
00271        int label = -1;
00272        for (boost::tie(i, end) = vertices(this->graph_); i != end; i++) {
00273           if (this->graph_[*i].v_label == -1) {
00274              graph_[*i].v_label = ++label;
00275           }
00276           AdjacencyIterator ai, a_end;
00277           boost::tie(ai, a_end) = adjacent_vertices(*i, this->graph_);
00278           for (; ai != a_end; ++ai) {
00279              bool found = false;
00280              EdgeDescriptor e_descriptor;
00281              boost::tie(e_descriptor, found) = boost::edge(*i, *ai, this->graph_);
00282              if (found) {
00283                 EdgeValue edge_val = boost::get(
00284                    boost::edge_weight, this->graph_, e_descriptor);
00285                 float weights_ = edge_val;
00286                 if (weights_ < _threshold) {
00287                    remove_edge(e_descriptor, this->graph_);
00288                 } else {
00289                    if (this->graph_[*ai].v_label == -1) {
00290                       this->graph_[*ai].v_label = this->graph_[*i].v_label;
00291                    }
00292                 }
00293              }
00294           }
00295        }
00296 #ifdef DEBUG
00297        // this->printGraph(this->graph_);
00298        std::cout << "\nPRINT INFO. \n --Graph Size: "
00299                  << num_vertices(this->graph_) <<
00300           std::endl << "--Total Label: " << label << "\n\n";
00301 #endif  // DEBUG
00302     }
00303 
00304     int RegionAdjacencyGraph::getCommonNeigbour(
00305        const std::vector<int> &c1_neigbour,
00306        const std::vector<int> &c2_neigbour)
00307     {
00308        int commonIndex = -1;
00309        for (int j = 0; j < c1_neigbour.size(); j++) {
00310           int c1_val = c1_neigbour[j];
00311           for (int i = 0; i < c2_neigbour.size(); i++) {
00312              int c2_val = c2_neigbour[i];
00313              if (c1_val == c2_val) {
00314                 commonIndex = c1_val;
00315                 break;
00316              }
00317           }
00318        }
00319        return commonIndex;
00320     }
00321 
00322     void RegionAdjacencyGraph::getCloudClusterLabels(
00323        std::vector<int> &labelMD)
00324     {
00325        labelMD.clear();
00326        VertexIterator i, end;
00327        for (boost::tie(i, end) = vertices(this->graph_); i != end; ++i) {
00328           labelMD.push_back(static_cast<int>(this->graph_[*i].v_label));
00329        }
00330     }
00331 
00332     void RegionAdjacencyGraph::printGraph(
00333        const Graph &_graph)
00334     {
00335        VertexIterator i, end;
00336        for (boost::tie(i, end) = vertices(_graph); i != end; ++i) {
00337           AdjacencyIterator ai, a_end;
00338           boost::tie(ai, a_end) = adjacent_vertices(*i, _graph);
00339           std::cout << *i << "\t" << _graph[*i].v_label << std::endl;
00340        }
00341     }
00342 
00343     void RegionAdjacencyGraph::computeCloudClusterRPYHistogram(
00344        const pcl::PointCloud<PointT>::Ptr _cloud,
00345        const pcl::PointCloud<pcl::Normal>::Ptr _normal,
00346        cv::Mat &_histogram)
00347     {
00348        pcl::VFHEstimation<PointT,
00349                           pcl::Normal,
00350                           pcl::VFHSignature308> vfh;
00351        vfh.setInputCloud(_cloud);
00352        vfh.setInputNormals(_normal);
00353        pcl::search::KdTree<PointT>::Ptr tree(
00354           new pcl::search::KdTree<PointT>);
00355        vfh.setSearchMethod(tree);
00356        pcl::PointCloud<pcl::VFHSignature308>::Ptr _vfhs(
00357           new pcl::PointCloud<pcl::VFHSignature308>());
00358        vfh.compute(*_vfhs);
00359        _histogram = cv::Mat(sizeof(char), 308, CV_32F);
00360        for (int i = 0; i < _histogram.cols; i++) {
00361           _histogram.at<float>(0, i) = _vfhs->points[0].histogram[i];
00362        }
00363        float curvature_ = 0.0f;
00364        for (int i = 0; i < _normal->size(); i++) {
00365           curvature_ += _normal->points[i].curvature;
00366        }
00367        curvature_ /= static_cast<float>(_normal->size());
00368        cv::normalize(
00369           _histogram, _histogram, 0, 1, cv::NORM_MINMAX, -1, cv::Mat());
00370     }
00371 }  // namespace jsk_pcl_ros


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Tue Jul 2 2019 19:41:44