environment_plane_modeling_nodelet.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 #define BOOST_PARAMETER_MAX_ARITY 7
00037 
00038 #include "jsk_pcl_ros/environment_plane_modeling.h"
00039 #include <pcl/filters/extract_indices.h>
00040 #include <pcl/common/distances.h>
00041 #include <pcl_conversions/pcl_conversions.h>
00042 #include <pcl/filters/project_inliers.h>
00043 #include <pcl/surface/convex_hull.h>
00044 #include <pcl/filters/project_inliers.h>
00045 #include <jsk_recognition_msgs/SparseOccupancyGridArray.h>
00046 
00047 #include <pluginlib/class_list_macros.h>
00048 #include <geometry_msgs/PoseArray.h>
00049 #include "jsk_recognition_utils/geo_util.h"
00050 #include "jsk_pcl_ros/grid_map.h"
00051 #include <jsk_topic_tools/rosparam_utils.h>
00052 namespace jsk_pcl_ros
00053 {
00054   void EnvironmentPlaneModeling::onInit()
00055   {
00056     DiagnosticNodelet::onInit();
00057     
00058     srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00059     typename dynamic_reconfigure::Server<Config>::CallbackType f =
00060       boost::bind (&EnvironmentPlaneModeling::configCallback, this, _1, _2);
00061     srv_->setCallback (f);
00062 
00063     pnh_->param("complete_footprint_region", complete_footprint_region_, false);
00064     pub_debug_magnified_polygons_
00065       = pnh_->advertise<jsk_recognition_msgs::PolygonArray>(
00066         "debug/magnified_polygons", 1);
00067     pub_debug_convex_point_cloud_
00068       = pnh_->advertise<sensor_msgs::PointCloud2>(
00069         "debug/convex_cloud", 1);
00070     pub_debug_raw_grid_map_
00071       = pnh_->advertise<jsk_recognition_msgs::SimpleOccupancyGridArray>(
00072         "debug/raw_grid_map", 1);
00073     pub_debug_noeroded_grid_map_
00074       = pnh_->advertise<jsk_recognition_msgs::SimpleOccupancyGridArray>(
00075         "debug/noeroded_grid_map", 1);
00076     pub_debug_plane_coords_
00077       = pnh_->advertise<geometry_msgs::PoseArray>(
00078         "debug/plane_poses", 1);
00079     pub_debug_magnified_plane_coords_
00080       = pnh_->advertise<geometry_msgs::PoseArray>(
00081         "debug/magnified_plane_poses", 1);
00082     pub_grid_map_
00083       = pnh_->advertise<jsk_recognition_msgs::SimpleOccupancyGridArray>(
00084         "output", 1, true);
00085     pub_snapped_move_base_simple_goal_ = pnh_->advertise<geometry_msgs::PoseStamped>(
00086       "/footstep_simple/goal", 1);
00087     pub_non_plane_indices_ = pnh_->advertise<PCLIndicesMsg>(
00088       "output/non_plane_indices", 1);
00089     if (complete_footprint_region_) {
00090       tf_listener_ = TfListenerSingleton::getInstance();
00091           
00092       sub_leg_bbox_ = pnh_->subscribe(
00093         "input/leg_bounding_box", 1,
00094         &EnvironmentPlaneModeling::boundingBoxCallback, this);
00095       
00096       jsk_topic_tools::readVectorParameter(
00097         *pnh_, "footprint_frames", footprint_frames_);
00098       
00099     }
00100     sub_move_base_simple_goal_ = pnh_->subscribe(
00101       "/move_base_simple/goal", 1,
00102       &EnvironmentPlaneModeling::moveBaseSimpleGoalCallback, this);
00103     sub_cloud_.subscribe(*pnh_, "input", 1);
00104     sub_full_cloud_.subscribe(*pnh_, "input/full_cloud", 1);
00105     sub_indices_.subscribe(*pnh_, "input/indices", 1);
00106     sub_polygons_.subscribe(*pnh_, "input/polygons", 1);
00107     sub_coefficients_.subscribe(*pnh_, "input/coefficients", 1);
00108     sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
00109     sync_->connectInput(sub_cloud_, sub_full_cloud_, sub_polygons_,
00110                         sub_coefficients_, sub_indices_);
00111     sync_->registerCallback(
00112       boost::bind(&EnvironmentPlaneModeling::inputCallback,
00113                   this, _1, _2, _3, _4, _5));
00114 
00115     onInitPostProcess();
00116   }
00117 
00118   void EnvironmentPlaneModeling::configCallback(Config &config, uint32_t level)
00119   {
00120     boost::mutex::scoped_lock lock(mutex_);
00121     magnify_distance_ = config.magnify_distance;
00122     distance_threshold_ = config.distance_threshold;
00123     normal_threshold_ = config.normal_threshold;
00124     resolution_ = config.resolution;
00125     morphological_filter_size_ = config.morphological_filter_size;
00126     erode_filter_size_ = config.erode_filter_size;
00127     footprint_plane_angular_threshold_ = config.footprint_plane_angular_threshold;
00128     footprint_plane_distance_threshold_ = config.footprint_plane_distance_threshold;
00129   }
00130 
00131   void EnvironmentPlaneModeling::printInputData(
00132     const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
00133     const sensor_msgs::PointCloud2::ConstPtr& full_cloud_msg,
00134     const jsk_recognition_msgs::PolygonArray::ConstPtr& polygon_msg,
00135     const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr& coefficients_msg,
00136     const jsk_recognition_msgs::ClusterPointIndices::ConstPtr& indices_msg)
00137   {
00138     NODELET_INFO("Input data --");
00139     NODELET_INFO("  Number of points -- %d", cloud_msg->width * cloud_msg->height);
00140     NODELET_INFO("  Number of full points -- %d", full_cloud_msg->width * full_cloud_msg->height);
00141     NODELET_INFO("  Number of clusters: -- %lu", indices_msg->cluster_indices.size());
00142     NODELET_INFO("  Frame Id: %s", cloud_msg->header.frame_id.c_str());
00143     NODELET_INFO("  Complete Footprint: %s", complete_footprint_region_? "true": "false");
00144   } 
00145 
00146   bool EnvironmentPlaneModeling::isValidFrameIds(
00147     const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
00148     const sensor_msgs::PointCloud2::ConstPtr& full_cloud_msg,
00149     const jsk_recognition_msgs::PolygonArray::ConstPtr& polygon_msg,
00150     const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr& coefficients_msg,
00151     const jsk_recognition_msgs::ClusterPointIndices::ConstPtr& indices_msg)
00152   {
00153     std::string frame_id = cloud_msg->header.frame_id;
00154     if (full_cloud_msg->header.frame_id != frame_id) {
00155       return false;
00156     }
00157     if (polygon_msg->header.frame_id != frame_id) {
00158       return false;
00159     }
00160     for (size_t i = 0; i < polygon_msg->polygons.size(); i++) {
00161       if (polygon_msg->polygons[i].header.frame_id != frame_id) {
00162         return false;
00163       }
00164     }
00165     if (coefficients_msg->header.frame_id != frame_id) {
00166       return false;
00167     }
00168     for (size_t i = 0; i < coefficients_msg->coefficients.size(); i++) {
00169       if (coefficients_msg->coefficients[i].header.frame_id != frame_id) {
00170         return false;
00171       }
00172     }
00173     if (indices_msg->header.frame_id != frame_id) {
00174       return false;
00175     }
00176     for (size_t i = 0; i < indices_msg->cluster_indices.size(); i++) {
00177       if (indices_msg->cluster_indices[i].header.frame_id != frame_id) {
00178         return false;
00179       }
00180     }
00181     return true;
00182   }
00183 
00184   void EnvironmentPlaneModeling::moveBaseSimpleGoalCallback(
00185     const geometry_msgs::PoseStamped::ConstPtr& msg)
00186   {
00187     boost::mutex::scoped_lock lock(mutex_);
00188     if (latest_grid_maps_.size() == 0) {
00189       NODELET_WARN("not yet grid maps are available");
00190       return;
00191     }
00192 
00193     tf::StampedTransform tf_transform = lookupTransformWithDuration(
00194       tf_listener_,
00195       latest_global_header_.frame_id,
00196       msg->header.frame_id,
00197       latest_global_header_.stamp,
00198       ros::Duration(1.0));
00199     
00200     Eigen::Affine3f local_coords, transform;
00201     tf::poseMsgToEigen(msg->pose, local_coords);
00202     tf::transformTFToEigen(tf_transform, transform);
00203     Eigen::Affine3f global_coords = transform * local_coords;
00204 
00205     // lookup suitable grid
00206     double max_height = - DBL_MAX;
00207     GridPlane::Ptr max_grid;
00208     Eigen::Affine3f max_projected_coords = Eigen::Affine3f::Identity();
00209     for (size_t i = 0; i < latest_grid_maps_.size(); i++) {
00210       GridPlane::Ptr target_grid = latest_grid_maps_[i];
00211       Eigen::Affine3f projected_coords;
00212       target_grid->getPolygon()->projectOnPlane(global_coords, projected_coords);
00213       Eigen::Vector3f projected_point(projected_coords.translation());
00214       if (target_grid->isOccupiedGlobal(projected_point)) {
00215         double height = projected_point[2];
00216         if (max_height < height) {
00217           max_height = height;
00218           max_grid = target_grid;
00219           max_projected_coords = projected_coords;
00220         }
00221       }
00222     }
00223     if (max_grid) {
00224       // publish it
00225       geometry_msgs::PoseStamped ros_coords;
00226       tf::poseEigenToMsg(max_projected_coords, ros_coords.pose);
00227       ros_coords.header.stamp = msg->header.stamp;
00228       ros_coords.header.frame_id = latest_global_header_.frame_id;
00229       pub_snapped_move_base_simple_goal_.publish(ros_coords);
00230     }
00231     else {
00232       NODELET_ERROR("Failed to find corresponding grid");
00233     }
00234   }
00235   
00236   void EnvironmentPlaneModeling::boundingBoxCallback(
00237     const jsk_recognition_msgs::BoundingBox::ConstPtr& box)
00238   {
00239     boost::mutex::scoped_lock lock(mutex_);
00240     latest_leg_bounding_box_ = box;
00241   }
00242   
00243   void EnvironmentPlaneModeling::inputCallback(
00244     const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
00245     const sensor_msgs::PointCloud2::ConstPtr& full_cloud_msg,
00246     const jsk_recognition_msgs::PolygonArray::ConstPtr& polygon_msg,
00247     const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr& coefficients_msg,
00248     const jsk_recognition_msgs::ClusterPointIndices::ConstPtr& indices_msg)
00249   {
00250     boost::mutex::scoped_lock lock(mutex_);
00251     try {
00252       // check frame_id
00253       if (!isValidFrameIds(cloud_msg, full_cloud_msg, polygon_msg, coefficients_msg, indices_msg)) {
00254         NODELET_FATAL("frame_id is not correct");
00255         return;
00256       }
00257       if (complete_footprint_region_ && !latest_leg_bounding_box_) {
00258         NODELET_ERROR("Bounding Box for Footprint is not yet ready");
00259         return;
00260       }
00261       // first, print all the information about ~inputs
00262       printInputData(cloud_msg, full_cloud_msg, polygon_msg, coefficients_msg, indices_msg);
00263 
00264     
00265       pcl::PointCloud<pcl::PointNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointNormal>);
00266       pcl::fromROSMsg(*cloud_msg, *cloud);
00267 
00268       pcl::PointCloud<pcl::PointNormal>::Ptr full_cloud (new pcl::PointCloud<pcl::PointNormal>);
00269       pcl::fromROSMsg(*full_cloud_msg, *full_cloud);
00270     
00271       // convert to jsk_recognition_utils::ConvexPolygon
00272       std::vector<jsk_recognition_utils::ConvexPolygon::Ptr> convexes = convertToConvexPolygons(cloud, indices_msg, coefficients_msg);
00273       // magnify convexes
00274       std::vector<jsk_recognition_utils::ConvexPolygon::Ptr> magnified_convexes = magnifyConvexes(convexes);
00275       publishConvexPolygonsBoundaries(pub_debug_convex_point_cloud_, cloud_msg->header, magnified_convexes);
00276       // Publish magnified convexes for debug
00277       publishConvexPolygons(pub_debug_magnified_polygons_, cloud_msg->header, magnified_convexes);
00278       // publish pose_array for debug
00279       {
00280         geometry_msgs::PoseArray pose_array;
00281         pose_array.header = cloud_msg->header;
00282         for (size_t i = 0; i < convexes.size(); i++) {
00283           Eigen::Affine3f pose = convexes[i]->coordinates();
00284           geometry_msgs::Pose ros_pose;
00285           tf::poseEigenToMsg(pose, ros_pose);
00286           pose_array.poses.push_back(ros_pose);
00287         }
00288         pub_debug_plane_coords_.publish(pose_array);
00289       }
00290       {
00291         geometry_msgs::PoseArray pose_array;
00292         pose_array.header = cloud_msg->header;
00293         for (size_t i = 0; i < magnified_convexes.size(); i++) {
00294           Eigen::Affine3f pose = magnified_convexes[i]->coordinates();
00295           geometry_msgs::Pose ros_pose;
00296           tf::poseEigenToMsg(pose, ros_pose);
00297           pose_array.poses.push_back(ros_pose);
00298         }
00299         pub_debug_magnified_plane_coords_.publish(pose_array);
00300       }
00301       
00302       // build GridMaps
00303       std::set<int> non_plane_indices;
00304       std::vector<GridPlane::Ptr> raw_grid_planes = buildGridPlanes(full_cloud, magnified_convexes, non_plane_indices);
00305       
00306       publishGridMaps(pub_debug_raw_grid_map_, cloud_msg->header, raw_grid_planes);
00307       PCLIndicesMsg ros_non_plane_indices;
00308       ros_non_plane_indices.indices = std::vector<int>(non_plane_indices.begin(),
00309                                                        non_plane_indices.end());
00310       ros_non_plane_indices.header = cloud_msg->header;
00311       pub_non_plane_indices_.publish(ros_non_plane_indices);
00312       std::vector<GridPlane::Ptr> morphological_filtered_grid_planes
00313         = morphologicalFiltering(raw_grid_planes);
00314       
00315       publishGridMaps(pub_debug_noeroded_grid_map_, cloud_msg->header,
00316                       morphological_filtered_grid_planes);
00317       
00318       std::vector<GridPlane::Ptr> eroded_grid_planes
00319         = erodeFiltering(morphological_filtered_grid_planes);
00320       std::vector<GridPlane::Ptr> result_grid_planes;
00321       
00322       if (complete_footprint_region_) { // complete footprint region if needed
00323         result_grid_planes
00324           = completeFootprintRegion(cloud_msg->header,
00325                                     eroded_grid_planes);
00326       }
00327       else {
00328         result_grid_planes = eroded_grid_planes;
00329      }
00330 
00331       publishGridMaps(pub_grid_map_, cloud_msg->header,
00332                       result_grid_planes);
00333       
00334       latest_global_header_ = cloud_msg->header;
00335       latest_grid_maps_ = result_grid_planes;
00336     }
00337     catch (tf2::TransformException& e) {
00338       NODELET_ERROR("Failed to lookup transformation: %s", e.what());
00339     }
00340   }
00341 
00342   std::vector<GridPlane::Ptr> EnvironmentPlaneModeling::erodeFiltering(
00343       std::vector<GridPlane::Ptr>& grid_maps)
00344   {
00345     std::vector<GridPlane::Ptr> ret;
00346     for (size_t i = 0; i < grid_maps.size(); i++) {
00347       GridPlane::Ptr eroded_grid_map = grid_maps[i]->erode(erode_filter_size_);
00348       ret.push_back(eroded_grid_map);
00349     }
00350     return ret;
00351   }
00352   
00353   int EnvironmentPlaneModeling::lookupGroundPlaneForFootprint(
00354     const Eigen::Affine3f& pose, const std::vector<GridPlane::Ptr>& grid_maps)
00355   {
00356     Eigen::Vector3f foot_z = (pose.rotation() * Eigen::Vector3f::UnitZ()).normalized();
00357     Eigen::Vector3f foot_p(pose.translation());
00358     double min_distance = DBL_MAX;
00359     int min_index = -1;
00360     for (size_t i = 0; i < grid_maps.size(); i++) {
00361       GridPlane::Ptr grid = grid_maps[i];
00362       Eigen::Vector3f normal = grid->getPolygon()->getNormal();
00363       if (std::abs(normal.dot(foot_z)) > cos(footprint_plane_angular_threshold_)) {
00364         // compare distance
00365         if (grid->getPolygon()->distanceToPoint(foot_p) < footprint_plane_distance_threshold_) {
00366           Eigen::Vector3f foot_center(pose.translation());
00367           if (!grid->isOccupiedGlobal(foot_center)) {
00368             // check distance to point
00369             double d = grid->getPolygon()->distanceFromVertices(foot_center);
00370             if (d < min_distance) {
00371               min_distance = d;
00372               min_index = i;
00373             }
00374           }
00375           else {
00376             NODELET_INFO("Foot print is already occupied");
00377             return -1;
00378           }
00379           // NB: else break?
00380         }
00381       }
00382     }
00383     return min_index;
00384   }
00385 
00386   int EnvironmentPlaneModeling::lookupGroundPlaneForFootprint(
00387     const std::string& footprint_frame, const std_msgs::Header& header,
00388     const std::vector<GridPlane::Ptr>& grid_maps)
00389   {
00390     // first, lookup location of frames
00391     tf::StampedTransform transform
00392       = lookupTransformWithDuration(tf_listener_,
00393                                     header.frame_id, footprint_frame,
00394                                     header.stamp,
00395                                     ros::Duration(1.0));
00396     Eigen::Affine3f eigen_transform;
00397     tf::transformTFToEigen(transform, eigen_transform);
00398     // lookup ground plane for the foot
00399     return lookupGroundPlaneForFootprint(eigen_transform, grid_maps);
00400   }
00401 
00402   GridPlane::Ptr EnvironmentPlaneModeling::completeGridMapByBoundingBox(
00403       const jsk_recognition_msgs::BoundingBox::ConstPtr& box,
00404       const std_msgs::Header& header,
00405       GridPlane::Ptr grid_map)
00406   {
00407     // resolve tf
00408     tf::StampedTransform tf_transform = lookupTransformWithDuration(
00409       tf_listener_,
00410       header.frame_id,
00411       box->header.frame_id,
00412       header.stamp,
00413       ros::Duration(1.0));
00414     Eigen::Affine3f transform;
00415     tf::transformTFToEigen(tf_transform, transform);
00416     Eigen::Affine3f local_pose;
00417     tf::poseMsgToEigen(box->pose, local_pose);
00418     Eigen::Affine3f global_pose = transform * local_pose;
00419     std::vector<double> dimensions;
00420     dimensions.push_back(box->dimensions.x);
00421     dimensions.push_back(box->dimensions.y);
00422     dimensions.push_back(box->dimensions.z);
00423     jsk_recognition_utils::Cube::Ptr cube (new Cube(Eigen::Vector3f(global_pose.translation()),
00424                              Eigen::Quaternionf(global_pose.rotation()),
00425                              dimensions));
00426     GridPlane::Ptr completed_grid_map = grid_map->clone();
00427     completed_grid_map->fillCellsFromCube(*cube);
00428     return completed_grid_map;
00429   }
00430   
00431   std::vector<GridPlane::Ptr> EnvironmentPlaneModeling::completeFootprintRegion(
00432     const std_msgs::Header& header, std::vector<GridPlane::Ptr>& grid_maps)
00433   {
00434     try {
00435       std::vector<GridPlane::Ptr> completed_grid_maps(grid_maps.size());
00436       std::set<int> ground_plane_indices;
00437       for (size_t i = 0; i < footprint_frames_.size(); i++) {
00438         std::string footprint_frame = footprint_frames_[i];
00439         int grid_index = lookupGroundPlaneForFootprint(
00440           footprint_frame, header, grid_maps);
00441         if (grid_index != -1) {
00442           NODELET_INFO("Found ground plane for %s: %d", footprint_frame.c_str(), grid_index);
00443           ground_plane_indices.insert(grid_index);
00444         }
00445         else {
00446           NODELET_WARN("Cannnot find ground plane for %s: %d", footprint_frame.c_str(), grid_index);
00447         }
00448       }
00449       for (size_t i = 0; i < grid_maps.size(); i++) {
00450         if (ground_plane_indices.find(i) == ground_plane_indices.end()) {
00451           // It's not a ground plane, just copy the original
00452           completed_grid_maps[i] = grid_maps[i];
00453         }
00454         else {
00455           completed_grid_maps[i] = completeGridMapByBoundingBox(
00456             latest_leg_bounding_box_, header, grid_maps[i]);
00457         }
00458       }
00459       return completed_grid_maps;
00460     }
00461     catch (tf2::TransformException& e) {
00462       NODELET_FATAL("Failed to lookup transformation: %s", e.what());
00463       return std::vector<GridPlane::Ptr>();
00464     }
00465   }
00466   
00467   std::vector<GridPlane::Ptr> EnvironmentPlaneModeling::morphologicalFiltering(
00468     std::vector<GridPlane::Ptr>& raw_grid_maps)
00469   {
00470     std::vector<GridPlane::Ptr> ret;
00471     for (size_t i = 0; i < raw_grid_maps.size(); i++) {
00472       GridPlane::Ptr dilated_grid_map = raw_grid_maps[i]->dilate(morphological_filter_size_);
00473       GridPlane::Ptr eroded_grid_map = dilated_grid_map->erode(morphological_filter_size_);
00474       ret.push_back(eroded_grid_map);
00475     }
00476     return ret;
00477   }
00478 
00479   
00480   void EnvironmentPlaneModeling::publishConvexPolygonsBoundaries(
00481     ros::Publisher& pub,
00482     const std_msgs::Header& header,
00483     std::vector<jsk_recognition_utils::ConvexPolygon::Ptr>& convexes)
00484   {
00485     pcl::PointCloud<pcl::PointXYZ>::Ptr
00486       boundary_cloud (new pcl::PointCloud<pcl::PointXYZ>);
00487     for (size_t i = 0; i < convexes.size(); i++) {
00488       pcl::PointCloud<pcl::PointXYZ>::Ptr
00489         one_boundary_cloud (new pcl::PointCloud<pcl::PointXYZ>);
00490       convexes[i]->boundariesToPointCloud<pcl::PointXYZ>(
00491         *one_boundary_cloud);
00492       *boundary_cloud = *boundary_cloud + *one_boundary_cloud;
00493     }
00494     sensor_msgs::PointCloud2 ros_cloud;
00495     pcl::toROSMsg(*boundary_cloud, ros_cloud);
00496     ros_cloud.header = header;
00497     pub.publish(ros_cloud);
00498   }
00499   
00500   void EnvironmentPlaneModeling::publishGridMaps(
00501       ros::Publisher& pub,
00502       const std_msgs::Header& header,
00503       std::vector<GridPlane::Ptr>& grids)
00504   {
00505     jsk_recognition_msgs::SimpleOccupancyGridArray grid_array;
00506     grid_array.header = header;
00507     for (size_t i = 0; i < grids.size(); i++) {
00508       jsk_recognition_msgs::SimpleOccupancyGrid grid = grids[i]->toROSMsg();
00509       grid.header = header;
00510       grid_array.grids.push_back(grid);
00511     }
00512     pub.publish(grid_array);
00513   }
00514   
00515   
00516   std::vector<GridPlane::Ptr> EnvironmentPlaneModeling::buildGridPlanes(
00517     pcl::PointCloud<pcl::PointNormal>::Ptr& cloud,
00518     std::vector<jsk_recognition_utils::ConvexPolygon::Ptr> convexes,
00519     std::set<int>& non_plane_indices)
00520   {
00521     std::vector<GridPlane::Ptr> ret(convexes.size());
00522 //#pragma omp parallel for
00523     for (size_t i = 0; i < convexes.size(); i++) {
00524       GridPlane::Ptr grid(new GridPlane(convexes[i], resolution_));
00525       size_t num = grid->fillCellsFromPointCloud(cloud, distance_threshold_,
00526                                                  normal_threshold_,
00527                                                  non_plane_indices);
00528       NODELET_INFO("%lu plane contains %lu points",
00529                        i, num);
00530       ret[i] = grid;
00531     }
00532     return ret;
00533   }
00534 
00535   
00536 
00537   void EnvironmentPlaneModeling::publishConvexPolygons(
00538     ros::Publisher& pub,
00539     const std_msgs::Header& header,
00540     std::vector<jsk_recognition_utils::ConvexPolygon::Ptr>& convexes)
00541   {
00542     jsk_recognition_msgs::PolygonArray polygon_array;
00543     polygon_array.header = header;
00544     for (size_t i = 0; i < convexes.size(); i++) {
00545       geometry_msgs::PolygonStamped polygon;
00546       polygon.polygon = convexes[i]->toROSMsg();
00547       polygon.header = header;
00548       polygon_array.polygons.push_back(polygon);
00549     }
00550     pub.publish(polygon_array);
00551   }
00552 
00553   std::vector<jsk_recognition_utils::ConvexPolygon::Ptr> EnvironmentPlaneModeling::magnifyConvexes(
00554     std::vector<jsk_recognition_utils::ConvexPolygon::Ptr>& convexes)
00555   {
00556     std::vector<jsk_recognition_utils::ConvexPolygon::Ptr> ret(0);
00557     for (size_t i = 0; i < convexes.size(); i++) {
00558       jsk_recognition_utils::ConvexPolygon::Ptr vertices_convex(new jsk_recognition_utils::ConvexPolygon(convexes[i]->getVertices()));
00559       jsk_recognition_utils::ConvexPolygon::Ptr new_convex = vertices_convex->magnifyByDistance(magnify_distance_);
00560       // check orientation
00561       if (new_convex->getNormalFromVertices().dot(Eigen::Vector3f::UnitZ()) < 0) {
00562         new_convex = boost::make_shared<jsk_recognition_utils::ConvexPolygon>(new_convex->flipConvex());
00563       }
00564       ret.push_back(new_convex);
00565     }
00566     return ret;
00567   }
00568   
00569   std::vector<jsk_recognition_utils::ConvexPolygon::Ptr> EnvironmentPlaneModeling::convertToConvexPolygons(
00570     const pcl::PointCloud<pcl::PointNormal>::Ptr& cloud,
00571     const jsk_recognition_msgs::ClusterPointIndices::ConstPtr& indices_msg,
00572     const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr& coefficients_msg)
00573   {
00574     std::vector<jsk_recognition_utils::ConvexPolygon::Ptr> convexes(0);
00575     for (size_t i = 0; i < indices_msg->cluster_indices.size(); i++) {
00576       pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
00577       inliers->indices = indices_msg->cluster_indices[i].indices;
00578       pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
00579       coefficients->values = coefficients_msg->coefficients[i].values;
00580       jsk_recognition_utils::ConvexPolygon::Ptr convex
00581         = jsk_recognition_utils::convexFromCoefficientsAndInliers<pcl::PointNormal>(
00582           cloud, inliers, coefficients);
00583       convexes.push_back(convex);
00584     }
00585     
00586     return convexes;
00587   }
00588   
00589 }
00590 
00591 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::EnvironmentPlaneModeling,
00592                         nodelet::Nodelet);


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