edgebased_cube_finder_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 #include "jsk_pcl_ros/edgebased_cube_finder.h"
00037 #include <pcl/kdtree/kdtree_flann.h>
00038 #include <pcl/common/centroid.h>
00039 #include "jsk_recognition_utils/pcl_conversion_util.h"
00040 #include <jsk_recognition_msgs/BoundingBoxArray.h>
00041 #include <geometry_msgs/PoseArray.h>
00042 #include <pcl/filters/extract_indices.h>
00043 #include <visualization_msgs/Marker.h>
00044 #include <eigen_conversions/eigen_msg.h>
00045 #include <pcl/sample_consensus/method_types.h>
00046 #include <pcl/sample_consensus/model_types.h>
00047 #include <pcl/segmentation/sac_segmentation.h>
00048 #include <pcl/common/angles.h>
00049 #include <jsk_recognition_msgs/PolygonArray.h>
00050 #include <jsk_recognition_msgs/ClusterPointIndices.h>
00051 #include <pcl/filters/project_inliers.h>
00052 #include <pcl/surface/convex_hull.h>
00053 #include <pcl_conversions/pcl_conversions.h>
00054 
00055 namespace jsk_pcl_ros
00056 {
00057   CubeHypothesis::CubeHypothesis(const IndicesPair& pair,
00058                                  const CoefficientsPair& coefficients_pair,
00059                                  const double outlier_threshold):
00060     value_(0.0), indices_pair_(pair), coefficients_pair_(coefficients_pair),
00061     outlier_threshold_(outlier_threshold_)
00062   {
00063 
00064   }
00065   
00066   CubeHypothesis::~CubeHypothesis()
00067   {
00068   }
00069 
00070   void CubeHypothesis::computeCentroid(
00071     const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud,
00072     const pcl::PointIndices::Ptr& indices,
00073     Eigen::Vector3f& output)
00074   {
00075     Eigen::Vector4f centroid;
00076     //pcl::compute3DCentroid(cloud, indices.indices, centroid);
00077     pcl::PointCloud<pcl::PointXYZRGB>::Ptr target_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
00078     pcl::ExtractIndices<pcl::PointXYZRGB> ex;
00079     ex.setInputCloud(cloud);
00080     ex.setIndices(indices);
00081     ex.filter(*target_cloud);
00082     pcl::compute3DCentroid(*target_cloud, centroid);
00083     jsk_recognition_utils::pointFromVectorToVector<Eigen::Vector4f, Eigen::Vector3f>(centroid, output);
00084   }
00085 
00086   void CubeHypothesis::getLinePoints(
00087     const jsk_recognition_utils::Line& line,
00088     const pcl::PointCloud<pcl::PointXYZRGB>& cloud,
00089     const pcl::PointIndices::Ptr indices,
00090     jsk_recognition_utils::Vertices& output)
00091   {
00092     pcl::PointCloud<pcl::PointXYZRGB>::Ptr points
00093       (new pcl::PointCloud<pcl::PointXYZRGB>);
00094     pcl::ExtractIndices<pcl::PointXYZRGB> extract;
00095     extract.setInputCloud(cloud.makeShared());
00096     extract.setIndices(indices);
00097     extract.filter(*points);
00098     for (size_t i = 0; i < points->points.size(); i++) {
00099       pcl::PointXYZRGB p = points->points[i];
00100       Eigen::Vector3f p_eigen = p.getVector3fMap();
00101       Eigen::Vector3f foot_point;
00102       line.foot(p_eigen, foot_point);
00103       output.push_back(foot_point);
00104     }
00105   }
00106 
00107   jsk_recognition_utils::ConvexPolygon::Ptr CubeHypothesis::buildConvexPolygon(
00108     const jsk_recognition_utils::PointPair& a_edge_pair, const jsk_recognition_utils::PointPair& b_edge_pair)
00109   {
00110     jsk_recognition_utils::Vertices vertices;
00111     vertices.push_back(a_edge_pair.get<0>());
00112     vertices.push_back(a_edge_pair.get<1>());
00113     vertices.push_back(b_edge_pair.get<1>());
00114     vertices.push_back(b_edge_pair.get<0>());
00115     jsk_recognition_utils::ConvexPolygon::Ptr convex (new jsk_recognition_utils::ConvexPolygon(vertices));
00116     return convex;
00117   }
00118   
00119   double CubeHypothesis::evaluatePointOnPlanes(
00120     const pcl::PointCloud<pcl::PointXYZRGB>& cloud,
00121     jsk_recognition_utils::ConvexPolygon& polygon_a,
00122     jsk_recognition_utils::ConvexPolygon& polygon_b)
00123   {
00124     std::vector<int> a_indices, b_indices;
00125     for (size_t i = 0; i < cloud.points.size(); i++) {
00126       pcl::PointXYZRGB pcl_point = cloud.points[i];
00127       if (pcl_isfinite(pcl_point.x) &&
00128           pcl_isfinite(pcl_point.y) &&
00129           pcl_isfinite(pcl_point.z)) { // we don't care nan points
00130         Eigen::Vector3f eigen_point = pcl_point.getVector3fMap();
00131         if (polygon_a.distanceSmallerThan(eigen_point, outlier_threshold_)) {
00132           a_indices.push_back(i);
00133         }
00134         if (polygon_b.distanceSmallerThan(eigen_point, outlier_threshold_)) {
00135           b_indices.push_back(i);
00136         }
00137       }
00138     }
00139     // ...the number of points...?
00140     return a_indices.size() + b_indices.size();
00141   }
00142   
00143   jsk_recognition_utils::PointPair CubeHypothesis::computeAxisEndPoints(
00144     const jsk_recognition_utils::Line& axis,
00145     const jsk_recognition_utils::PointPair& a_candidates,
00146     const jsk_recognition_utils::PointPair& b_candidates)
00147   {
00148     jsk_recognition_utils::Vertices original_points;
00149     original_points.push_back(a_candidates.get<0>());
00150     original_points.push_back(a_candidates.get<1>());
00151     original_points.push_back(b_candidates.get<0>());
00152     original_points.push_back(b_candidates.get<1>());
00153     for (size_t i = 0; i < original_points.size(); i++) {
00154       Eigen::Vector3f p = original_points[i];
00155       ROS_INFO("[foot_point] [%f, %f, %f]", p[0], p[1], p[2]);
00156     }
00157     
00158     jsk_recognition_utils::Vertices foot_points;
00159     for (size_t i = 0; i < original_points.size(); i++) {
00160       Eigen::Vector3f foot_point;
00161       axis.foot(original_points[i], foot_point);
00162       foot_points.push_back(foot_point);
00163     }
00164     double max_alpha = -DBL_MAX;
00165     double min_alpha = DBL_MAX;
00166     Eigen::Vector3f max_alpha_point, min_alpha_point;
00167     
00168     for (size_t i = 0; i < foot_points.size(); i++) {
00169       double alpha = axis.computeAlpha(foot_points[i]);
00170       if (alpha > max_alpha) {
00171         max_alpha = alpha;
00172         max_alpha_point = foot_points[i];
00173       }
00174       if (alpha < min_alpha) {
00175         min_alpha = alpha;
00176         min_alpha_point = foot_points[i];
00177       }
00178     }
00179     ROS_INFO("min_alpha_point: [%f, %f, %f]", min_alpha_point[0], min_alpha_point[1], min_alpha_point[2]);
00180     ROS_INFO("max_alpha_point: [%f, %f, %f]", max_alpha_point[0], max_alpha_point[1], max_alpha_point[2]);
00181     return boost::make_tuple(min_alpha_point, max_alpha_point);
00182   }
00183 
00184   
00185   PlanarCubeHypothesis::PlanarCubeHypothesis(
00186     const IndicesPair& pair, const CoefficientsPair& coefficients_pair, const double outlier_threshold):
00187     CubeHypothesis(pair, coefficients_pair, outlier_threshold)
00188   {
00189 
00190   }
00191   
00192   DiagnoalCubeHypothesis::DiagnoalCubeHypothesis(
00193     const IndicesPair& pair, const CoefficientsPair& coefficients_pair,
00194     const double outlier_threshold):
00195     CubeHypothesis(pair, coefficients_pair, outlier_threshold), resolution_(10)
00196   {
00197 
00198   }
00199 
00200   void DiagnoalCubeHypothesis::estimate(
00201     const pcl::PointCloud<pcl::PointXYZRGB>& cloud)
00202   {
00203     const double dt = (M_PI - 2.0 * min_angle_) / resolution_;
00204     jsk_recognition_utils::Line::Ptr line_a
00205       = jsk_recognition_utils::Line::fromCoefficients(coefficients_pair_.get<0>()->values);
00206     jsk_recognition_utils::Line::Ptr line_b
00207       = jsk_recognition_utils::Line::fromCoefficients(coefficients_pair_.get<1>()->values);
00208     if (!line_a->isSameDirection(*line_b)) {
00209       line_b = line_b->flip();
00210     }
00211     
00212     const double r2 = line_a->distance(*line_b);
00213     const double r = r2 / 2;
00214     jsk_recognition_utils::Line::Ptr axis = line_a->midLine(*line_b);
00215     Eigen::Vector3f center;
00216     axis->getOrigin(center);
00217     ROS_INFO("line_a:");
00218     line_a->print();
00219     ROS_INFO("line_b:");
00220     line_b->print();
00221     ROS_INFO("axis:");
00222     axis->print();
00223     ROS_INFO("r: %f", r);
00224     
00225     // before evaluation, we fix line_a and line_b to be parallel
00226     // against line_c and axis.
00227     // in order to align line_a and line_b, we compute centroids of the points
00228     // on line_a and line_b and rotate the lines around the points.
00229     Eigen::Vector3f centroid_a, centroid_b;
00230     pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_ptr = cloud.makeShared();
00231     computeCentroid(cloud_ptr, indices_pair_.get<0>(), centroid_a);
00232     computeCentroid(cloud_ptr, indices_pair_.get<1>(), centroid_b);
00233     ROS_INFO("centroid_a: [%f, %f, %f]", centroid_a[0], centroid_a[1], centroid_a[2]);
00234     ROS_INFO("centroid_b: [%f, %f, %f]", centroid_b[0], centroid_b[1], centroid_b[2]);
00235     jsk_recognition_utils::Line::Ptr line_a_aligned = axis->parallelLineOnAPoint(centroid_a);
00236     jsk_recognition_utils::Line::Ptr line_b_aligned = axis->parallelLineOnAPoint(centroid_b);
00237     ROS_INFO("line_a_aligned:");
00238     line_a_aligned->print();
00239     ROS_INFO("line_b_aligned:");
00240     line_b_aligned->print();
00241     
00242     jsk_recognition_utils::Vertices line_a_points, line_b_points;
00243     getLinePoints(*line_a_aligned, cloud, indices_pair_.get<0>(),
00244                   line_a_points);
00245     getLinePoints(*line_b_aligned, cloud, indices_pair_.get<1>(),
00246                   line_b_points);
00247     jsk_recognition_utils::PointPair line_a_end_points = line_a->findEndPoints(line_a_points);
00248     jsk_recognition_utils::PointPair line_b_end_points = line_b->findEndPoints(line_b_points);
00249     double max_v = - DBL_MAX;
00250     double max_theta;
00251     jsk_recognition_utils::Line::Ptr max_line_c;
00252     jsk_recognition_utils::PointPair max_line_c_a_points, max_line_c_b_points;
00253     for (size_t i = 0; i < resolution_; i++) {
00254       ROS_INFO("estimate i: %lu", i);
00255       double theta = dt * i + min_angle_;
00256       Eigen::Vector3f point_on_x;
00257       line_a->foot(center, point_on_x);
00258       // 2D local cooridnate: r * cos(theta), r * sin(theta)
00259       // convert it according to axis
00260       // x // (point_on_x - center)
00261       // z // axis
00262       Eigen::Vector3f ex = (point_on_x - center).normalized();
00263       Eigen::Vector3f ez;
00264       axis->getDirection(ez);
00265       Eigen::Vector3f ey = ez.cross(ex).normalized();
00266       // ey should direct to origin. If not, ex should be flipped
00267       if (center.dot(ey) > 0) {
00268         ex = -ex;
00269         ey = ez.cross(ex).normalized();
00270       }
00271       Eigen::Vector3f point_on_y = center + r * ey;
00272       jsk_recognition_utils::Line::Ptr line_c = axis->parallelLineOnAPoint(point_on_y);
00273       // line_a, b, c are almost parallel and these 3 lines make
00274       // 2 planes.
00275       //  ------------------------ line_a
00276       // /         A            /
00277       // ----------------------- line_c
00278       // \         B            \
00279       //  ----------------------- line_b
00280       
00281       // in order to evaluate plane A, first we exract the points
00282       // which have distance |line_a - line_c| + margin from line_c.
00283       // Second, For all those points, count all the points whose distance from
00284       // plane A is smaller than margin.
00285       // Thrid, we compute the area of the plane A and normalize the value.
00286       Eigen::Vector3f line_c_a_min_point, line_c_a_max_point;
00287       Eigen::Vector3f line_c_b_min_point, line_c_b_max_point;
00288       line_c->foot(line_a_end_points.get<0>(), line_c_a_min_point);
00289       line_c->foot(line_a_end_points.get<1>(), line_c_a_max_point);
00290       line_c->foot(line_b_end_points.get<0>(), line_c_b_min_point);
00291       line_c->foot(line_b_end_points.get<1>(), line_c_b_max_point);
00292       jsk_recognition_utils::PointPair line_c_a_end_points = boost::make_tuple(line_c_a_min_point,
00293                                                         line_c_a_max_point);
00294       jsk_recognition_utils::PointPair line_c_b_end_points = boost::make_tuple(line_c_b_min_point,
00295                                                         line_c_b_max_point);
00296       jsk_recognition_utils::ConvexPolygon::Ptr plane_a = buildConvexPolygon(line_a_end_points,
00297                                                       line_c_a_end_points);
00298       jsk_recognition_utils::ConvexPolygon::Ptr plane_b = buildConvexPolygon(line_b_end_points,
00299                                                       line_c_b_end_points);
00300       double v = evaluatePointOnPlanes(cloud, *plane_a, *plane_b);
00301       if (max_v < v) {
00302         max_v = v;
00303         max_theta = theta;
00304         max_line_c = line_c;
00305         max_line_c_a_points = line_c_a_end_points;
00306         max_line_c_b_points = line_c_b_end_points;
00307       } 
00308    }
00309     value_ = max_v;
00310     // estimate centroid
00311     jsk_recognition_utils::PointPair axis_end_points = computeAxisEndPoints(
00312       *axis,
00313       max_line_c_a_points,
00314       max_line_c_b_points);
00315     ROS_INFO("end_point: [%f, %f, %f]", axis_end_points.get<0>()[0], axis_end_points.get<0>()[1], axis_end_points.get<0>()[2]);
00316     ROS_INFO("end_point: [%f, %f, %f]", axis_end_points.get<1>()[0], axis_end_points.get<1>()[1], axis_end_points.get<1>()[2]);
00317     Eigen::Vector3f midpoint
00318       = (axis_end_points.get<0>() + axis_end_points.get<1>()) / 2.0;
00319     double z_dimension = (axis_end_points.get<0>() - midpoint).norm() * 2;
00320     // compute cube
00321     ROS_INFO("midpoint: [%f, %f, %f]", midpoint[0], midpoint[1], midpoint[2]);
00322     cube_.reset(new jsk_recognition_utils::Cube(midpoint, *line_a_aligned, *line_b_aligned, *max_line_c));
00323     std::vector<double> dimensions = cube_->getDimensions();
00324     dimensions[2] = z_dimension;
00325     cube_->setDimensions(dimensions);
00326   }
00327 
00328   int EdgebasedCubeFinder::countInliers(
00329     const pcl::PointCloud<PointT>::Ptr cloud,
00330     const jsk_recognition_utils::ConvexPolygon::Ptr convex)
00331   {
00332     int num = 0;
00333     for (size_t i = 0; i < cloud->points.size(); i++) {
00334       PointT p = cloud->points[i];
00335       if (!std::isnan(p.x) && !std::isnan(p.y) && !std::isnan(p.z)) {
00336         Eigen::Vector3f ep = p.getVector3fMap();
00337         if (convex->distanceSmallerThan(ep, outlier_threshold_)) {
00338           num++;
00339         }
00340       }
00341     }
00342     return num;
00343   }
00344   
00345   void EdgebasedCubeFinder::filterBasedOnConvex(
00346     const pcl::PointCloud<PointT>::Ptr cloud,
00347     const std::vector<jsk_recognition_utils::ConvexPolygon::Ptr>& convexes,
00348     std::vector<int>& output_indices)
00349   {
00350     
00351     for (size_t i = 0; i < convexes.size(); i++) {
00352       jsk_recognition_utils::ConvexPolygon::Ptr convex = convexes[i];
00353       if (true) {
00354         // if (convex->area() > convex_area_threshold_ &&
00355         //     convex->allEdgesLongerThan(convex_edge_threshold_)) {
00356         //int inliers = countInliers(cloud, convex);
00357         //ROS_INFO("inliers: %d", inliers);
00358         //if (inliers > min_inliers_) {
00359         if (true) {
00360           output_indices.push_back(i);
00361         }
00362       }
00363     }
00364   }
00365 
00366   void EdgebasedCubeFinder::configCallback (Config &config, uint32_t level)
00367   {
00368     boost::mutex::scoped_lock lock(mutex_);
00369     outlier_threshold_ = config.outlier_threshold;
00370     min_inliers_ = config.min_inliers;
00371     convex_area_threshold_ = config.convex_area_threshold;
00372     convex_edge_threshold_ = config.convex_edge_threshold;
00373     parallel_edge_distance_min_threshold_ = config.parallel_edge_distance_min_threshold;
00374     parallel_edge_distance_max_threshold_ = config.parallel_edge_distance_max_threshold;
00375   }
00376   
00377   void EdgebasedCubeFinder::onInit()
00378   {
00379     ConnectionBasedNodelet::onInit();
00380     
00381 
00382     srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00383     dynamic_reconfigure::Server<Config>::CallbackType f =
00384       boost::bind (&EdgebasedCubeFinder::configCallback, this, _1, _2);
00385     srv_->setCallback (f);
00386 
00387     
00389     // publishers
00391     pub_ = advertise<jsk_recognition_msgs::BoundingBoxArray>(*pnh_, "output", 1);
00392     pub_pose_array_
00393       = advertise<geometry_msgs::PoseArray>(*pnh_, "output_pose_array", 1);
00394     pub_debug_marker_
00395       = advertise<visualization_msgs::Marker>(*pnh_, "debug_marker", 1);
00396     pub_debug_filtered_cloud_ = advertise<sensor_msgs::PointCloud2>(
00397       *pnh_, "debug_filtered_cloud", 1);
00398     pub_debug_polygons_
00399       = advertise<jsk_recognition_msgs::PolygonArray>(*pnh_, "debug_polygons", 1);
00400     pub_debug_clusers_
00401       = advertise<jsk_recognition_msgs::ClusterPointIndices>(*pnh_, "debug_clusters", 1);
00402 
00403     onInitPostProcess();
00404   }
00405 
00406   void EdgebasedCubeFinder::subscribe()
00407   {
00409     // subscription
00411     sub_input_.subscribe(*pnh_, "input", 1);
00412     sub_edges_.subscribe(*pnh_, "input_edges", 1);
00413     sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
00414     sync_->connectInput(sub_input_, sub_edges_);
00415     sync_->registerCallback(boost::bind(
00416                               &EdgebasedCubeFinder::estimate, this, _1, _2));
00417   }
00418 
00419   void EdgebasedCubeFinder::unsubscribe()
00420   {
00421     sub_input_.unsubscribe();
00422     sub_edges_.unsubscribe();
00423   }
00424 
00425   jsk_recognition_utils::Line::Ptr EdgebasedCubeFinder::midLineFromCoefficientsPair(
00426     const CoefficientsPair& pair)
00427   {
00428     pcl::ModelCoefficients::Ptr coefficients_a = pair.get<0>();
00429     pcl::ModelCoefficients::Ptr coefficients_b = pair.get<1>();
00430     jsk_recognition_utils::Line::Ptr line_a = jsk_recognition_utils::Line::fromCoefficients(coefficients_a->values);
00431     jsk_recognition_utils::Line::Ptr line_b = jsk_recognition_utils::Line::fromCoefficients(coefficients_b->values);
00432     return line_a->midLine(*line_b);
00433   }
00434 
00435   pcl::PointCloud<EdgebasedCubeFinder::PointT>::Ptr EdgebasedCubeFinder::extractPointCloud(
00436     const pcl::PointCloud<PointT>::Ptr cloud,
00437     const pcl::PointIndices::Ptr indices)
00438   {
00439     pcl::PointCloud<PointT>::Ptr ret (new pcl::PointCloud<PointT>);
00440     pcl::ExtractIndices<PointT> ex;
00441     ex.setInputCloud(cloud);
00442     ex.setIndices(indices);
00443     ex.filter(*ret);
00444     return ret;
00445   }
00446 
00447   jsk_recognition_utils::PointPair EdgebasedCubeFinder::minMaxPointOnLine(
00448     const jsk_recognition_utils::Line& line,
00449     const pcl::PointCloud<PointT>::Ptr cloud)
00450   {
00451     jsk_recognition_utils::Vertices points;
00452     for (size_t i = 0; i < cloud->points.size(); i++) {
00453       PointT p = cloud->points[i];
00454       Eigen::Vector3f eigen_p = p.getVector3fMap();
00455       Eigen::Vector3f foot;
00456       line.foot(eigen_p, foot);
00457       points.push_back(foot);
00458     }
00459     return line.findEndPoints(points);
00460   }
00461   
00462   jsk_recognition_utils::ConvexPolygon::Ptr EdgebasedCubeFinder::convexFromPairs(
00463     const pcl::PointCloud<PointT>::Ptr cloud,
00464     const CoefficientsPair& coefficients_pair,
00465     const IndicesPair& indices_pair)
00466   {
00467     pcl::ModelCoefficients::Ptr coefficients_a = coefficients_pair.get<0>();
00468     pcl::ModelCoefficients::Ptr coefficients_b = coefficients_pair.get<1>();
00469     pcl::PointIndices::Ptr indices_a = indices_pair.get<0>();
00470     pcl::PointIndices::Ptr indices_b = indices_pair.get<1>();
00471     
00472     pcl::PointCloud<PointT>::Ptr cloud_a = extractPointCloud(cloud, indices_a);
00473     pcl::PointCloud<PointT>::Ptr cloud_b = extractPointCloud(cloud, indices_b);
00474 
00475     jsk_recognition_utils::Line::Ptr line_a = jsk_recognition_utils::Line::fromCoefficients(coefficients_a->values);
00476     jsk_recognition_utils::Line::Ptr line_b = jsk_recognition_utils::Line::fromCoefficients(coefficients_b->values);
00477     jsk_recognition_utils::PointPair a_min_max = minMaxPointOnLine(*line_a, cloud_a);
00478     jsk_recognition_utils::PointPair b_min_max = minMaxPointOnLine(*line_b, cloud_b);
00479     jsk_recognition_utils::Vertices vertices;
00480     vertices.push_back(a_min_max.get<0>());
00481     vertices.push_back(a_min_max.get<1>());
00482     vertices.push_back(b_min_max.get<1>());
00483     vertices.push_back(b_min_max.get<0>());
00484     jsk_recognition_utils::ConvexPolygon::Ptr ret (new jsk_recognition_utils::ConvexPolygon(vertices));
00485     return ret;
00486   }
00487   
00488   void EdgebasedCubeFinder::filterPairsBasedOnParallelEdgeDistances(
00489     const std::vector<IndicesPair>& pairs,
00490     const std::vector<CoefficientsPair>& coefficients_pair,
00491     std::vector<IndicesPair>& filtered_indices_pairs,
00492     std::vector<CoefficientsPair>& filtered_coefficients_pairs)
00493   {
00494     for (size_t i = 0; i < coefficients_pair.size(); i++) {
00495       CoefficientsPair coefficients = coefficients_pair[i];
00496       pcl::ModelCoefficients::Ptr coefficients_a = coefficients_pair[i].get<0>();
00497       pcl::ModelCoefficients::Ptr coefficients_b = coefficients_pair[i].get<1>();
00498       jsk_recognition_utils::Line::Ptr line_a = jsk_recognition_utils::Line::fromCoefficients(coefficients_a->values);
00499       jsk_recognition_utils::Line::Ptr line_b = jsk_recognition_utils::Line::fromCoefficients(coefficients_b->values);
00500 
00501       // force to align two lines
00502       jsk_recognition_utils::Line::Ptr axis = line_a->midLine(*line_b);
00503       Eigen::Vector3f origin_a, origin_b;
00504       line_a->getOrigin(origin_a);
00505       line_b->getOrigin(origin_b);
00506       jsk_recognition_utils::Line::Ptr line_a_aligned = axis->parallelLineOnAPoint(origin_a);
00507       jsk_recognition_utils::Line::Ptr line_b_aligned = axis->parallelLineOnAPoint(origin_b);
00508       Eigen::Vector3f distance_vector;
00509       line_a_aligned->parallelLineNormal(*line_b_aligned, distance_vector);
00510       double distance = distance_vector.norm();
00511       //double distance = line_a_aligned->distance(*line_b_aligned);
00512       ROS_INFO("d: %f", distance);
00513       if (distance < parallel_edge_distance_max_threshold_ &&
00514           distance > parallel_edge_distance_min_threshold_) {
00515         filtered_indices_pairs.push_back(pairs[i]);
00516         filtered_coefficients_pairs.push_back(coefficients);
00517       }
00518     }
00519   }
00520 
00521   //pcl::PointCloud<EdgebasedCubeFinder::PointT>::Ptr
00522   pcl::PointIndices::Ptr
00523   EdgebasedCubeFinder::preparePointCloudForRANSAC(
00524     const jsk_recognition_utils::ConvexPolygon::Ptr convex,
00525     const CoefficientsPair& edge_coefficients_pair,
00526     const pcl::PointCloud<PointT>::Ptr cloud)
00527   {
00528     // extract the points which can be projected onto the convex.
00529     pcl::PointIndices::Ptr indices(new pcl::PointIndices);
00530     jsk_recognition_utils::ConvexPolygon::Ptr magnified_convex = convex->magnify(1.1);
00531     pcl::PointCloud<PointT>::Ptr ret (new pcl::PointCloud<PointT>);
00532     for (size_t i = 0; i < cloud->points.size(); i++) {
00533       PointT p = cloud->points[i];
00534       if (!std::isnan(p.x) && !std::isnan(p.y) && !std::isnan(p.z)) {
00535         Eigen::Vector3f ep = p.getVector3fMap();
00536         Eigen::Vector3f foot;
00537         magnified_convex->projectOnPlane(ep, foot);
00538         if (magnified_convex->isInside(foot) && convex->distanceSmallerThan(ep, outlier_threshold_)) {
00539         //if (magnified_convex->isInside(foot) && (ep - foot).norm()) {
00540           //NODELET_INFO("distance: %f", (ep - foot).norm());
00541           indices->indices.push_back(i);
00542         }
00543       }
00544     }
00545     return indices;
00546    }
00547 
00548    void EdgebasedCubeFinder::estimateParallelPlane(
00549      const jsk_recognition_utils::ConvexPolygon::Ptr convex,
00550      const pcl::PointCloud<PointT>::Ptr filtered_cloud,
00551      pcl::PointIndices::Ptr output_inliers,
00552      pcl::ModelCoefficients::Ptr output_coefficients)
00553    {
00554      Eigen::Vector3f normal = convex->getNormal();
00555      pcl::SACSegmentation<PointT> seg;
00556      seg.setOptimizeCoefficients (true);
00557      seg.setModelType (pcl::SACMODEL_PERPENDICULAR_PLANE);
00558      seg.setMethodType (pcl::SAC_RANSAC);
00559      seg.setDistanceThreshold (outlier_threshold_);
00560      seg.setInputCloud(filtered_cloud);
00561      seg.setMaxIterations (10000);
00562      seg.setAxis(normal);
00563      seg.setEpsAngle(pcl::deg2rad(10.0));
00564      seg.segment (*output_inliers, *output_coefficients);
00565    }
00566 
00567    void EdgebasedCubeFinder::estimatePerpendicularPlane(
00568      const jsk_recognition_utils::ConvexPolygon::Ptr convex,
00569      const CoefficientsPair& edge_coefficients,
00570      const pcl::PointCloud<PointT>::Ptr filtered_cloud,
00571      pcl::PointIndices::Ptr output_inliers,
00572      pcl::ModelCoefficients::Ptr output_coefficients)
00573    {
00574      Eigen::Vector3f normal_a = convex->getNormal();
00575      jsk_recognition_utils::Line::Ptr mid_line = midLineFromCoefficientsPair(edge_coefficients);
00576      Eigen::Vector3f normal_b;
00577      mid_line->getDirection(normal_b);
00578      Eigen::Vector3f normal = normal_a.cross(normal_b);
00579      pcl::SACSegmentation<PointT> seg;
00580      seg.setOptimizeCoefficients (true);
00581      seg.setModelType (pcl::SACMODEL_PERPENDICULAR_PLANE);
00582      seg.setMethodType (pcl::SAC_RANSAC);
00583      seg.setDistanceThreshold (outlier_threshold_);
00584      seg.setInputCloud(filtered_cloud);
00585      seg.setMaxIterations (10000);
00586      seg.setAxis(normal);
00587      seg.setEpsAngle(pcl::deg2rad(5.0));
00588      seg.segment (*output_inliers, *output_coefficients);
00589    }
00590 
00591   jsk_recognition_utils::Cube::Ptr EdgebasedCubeFinder::cubeFromIndicesAndCoefficients(
00592     const pcl::PointCloud<EdgebasedCubeFinder::PointT>::Ptr cloud,
00593     const IndicesCoefficientsTriple& indices_coefficients_triple,
00594     pcl::PointCloud<EdgebasedCubeFinder::PointT>::Ptr points_on_edge)
00595   {
00596     Eigen::Vector3f ex, ey, ez;
00597     CoefficientsTriple coefficients_triple
00598       = indices_coefficients_triple.get<1>();
00599     IndicesTriple indices_triple
00600       = indices_coefficients_triple.get<0>();
00601     // do we need to align lines...??
00602     jsk_recognition_utils::Line::Ptr mid_line
00603       = jsk_recognition_utils::Line::fromCoefficients(coefficients_triple.get<0>()->values);
00604     jsk_recognition_utils::Line::Ptr line_a
00605       = jsk_recognition_utils::Line::fromCoefficients(coefficients_triple.get<1>()->values);
00606     jsk_recognition_utils::Line::Ptr line_b
00607       = jsk_recognition_utils::Line::fromCoefficients(coefficients_triple.get<2>()->values);
00608     // force to align
00609     if (!mid_line->isSameDirection(*line_a)) {
00610       line_a = line_a->flip();
00611     }
00612     if (!mid_line->isSameDirection(*line_b)) {
00613       line_b = line_b->flip();
00614     }
00615     jsk_recognition_utils::Line::Ptr axis = line_a->midLine(*line_b);
00616     
00617     pcl::PointCloud<PointT>::Ptr point_on_a
00618       = extractPointCloud(cloud,
00619                           indices_triple.get<1>());
00620     pcl::PointCloud<PointT>::Ptr point_on_b
00621       = extractPointCloud(cloud,
00622                           indices_triple.get<2>());
00623     pcl::PointCloud<PointT>::Ptr point_on_c
00624       = extractPointCloud(cloud,
00625                           indices_triple.get<0>());
00626     Eigen::Vector4f a_centroid4, b_centroid4, c_centroid4;
00627     Eigen::Vector3f a_centroid, b_centroid, c_centroid;
00628     pcl::compute3DCentroid(*point_on_a, a_centroid4);
00629     pcl::compute3DCentroid(*point_on_b, b_centroid4);
00630     pcl::compute3DCentroid(*point_on_c, c_centroid4);
00631     jsk_recognition_utils::pointFromVectorToVector<Eigen::Vector4f, Eigen::Vector3f>(
00632       a_centroid4, a_centroid);
00633     jsk_recognition_utils::pointFromVectorToVector<Eigen::Vector4f, Eigen::Vector3f>(
00634       b_centroid4, b_centroid);
00635     jsk_recognition_utils::pointFromVectorToVector<Eigen::Vector4f, Eigen::Vector3f>(
00636       c_centroid4, c_centroid);
00637     
00638     jsk_recognition_utils::Line::Ptr line_a_aligned = axis->parallelLineOnAPoint(a_centroid);
00639     jsk_recognition_utils::Line::Ptr line_b_aligned = axis->parallelLineOnAPoint(b_centroid);
00640     jsk_recognition_utils::Line::Ptr mid_line_aligned = axis->parallelLineOnAPoint(c_centroid);
00641     //Line::Ptr axis_aligned = axis->parallelLineOnAPoint(c_centroid);
00642     pcl::PointCloud<PointT>::Ptr all_points(new pcl::PointCloud<PointT>);
00643     *all_points = *point_on_a + *point_on_b;
00644     *all_points = *all_points + *point_on_c;
00645     *points_on_edge = *all_points;
00646     
00647     // PointT a_centroid_p, b_centroid_p, c_centroid_p;
00648     // a_centroid_p.x = a_centroid[0];
00649     // a_centroid_p.y = a_centroid[1];
00650     // a_centroid_p.z = a_centroid[2];
00651     // b_centroid_p.x = b_centroid[0];
00652     // b_centroid_p.y = b_centroid[1];
00653     // b_centroid_p.z = b_centroid[2];
00654     // c_centroid_p.x = c_centroid[0];
00655     // c_centroid_p.y = c_centroid[1];
00656     // c_centroid_p.z = c_centroid[2];
00657     // points_on_edge->points.push_back(a_centroid_p);
00658     // points_on_edge->points.push_back(b_centroid_p);
00659     // points_on_edge->points.push_back(c_centroid_p);
00660       
00661     jsk_recognition_utils::PointPair min_max_points = minMaxPointOnLine(*axis, all_points);
00662     PointT min_point, max_point;
00663     // min_point.x = min_max_points.get<0>()[0];
00664     // min_point.y = min_max_points.get<0>()[1];
00665     // min_point.z = min_max_points.get<0>()[2];
00666     // max_point.x = min_max_points.get<1>()[0];
00667     // max_point.y = min_max_points.get<1>()[1];
00668     // max_point.z = min_max_points.get<1>()[2];
00669     // points_on_edge->points.push_back(min_point);
00670     // points_on_edge->points.push_back(max_point);
00671     Eigen::Vector3f center_point
00672       = (min_max_points.get<0>() + min_max_points.get<1>()) / 2.0;
00673     double z_width = (min_max_points.get<0>() - min_max_points.get<1>()).norm();
00674     mid_line_aligned->getDirection(ez);
00675     mid_line_aligned->parallelLineNormal(*line_a_aligned, ex);
00676     mid_line_aligned->parallelLineNormal(*line_b_aligned, ey);
00677     
00678     double x_width = ex.norm();
00679     double y_width = ey.norm();
00680     
00681     ex.normalize();
00682     ey.normalize();
00683     ez.normalize();
00684 
00685     ROS_INFO("ex: [%f, %f, %f]", ex[0], ex[1], ex[2]);
00686     ROS_INFO("ey: [%f, %f, %f]", ey[0], ey[1], ey[2]);
00687     ROS_INFO("ez: [%f, %f, %f]", ez[0], ez[1], ez[2]);
00688     
00689     if (ex.cross(ey).dot(ez) < 0) {
00690       ez = -ez;
00691     }
00692     
00693     Eigen::Quaternionf rot = jsk_recognition_utils::rotFrom3Axis(ex, ey, ez);
00694     std::vector<double> dimensions;
00695     dimensions.push_back(x_width);
00696     dimensions.push_back(y_width);
00697     dimensions.push_back(z_width);
00698     jsk_recognition_utils::Cube::Ptr ret (new jsk_recognition_utils::Cube(center_point, rot, dimensions));
00699     return ret;
00700   }
00701   
00702   void EdgebasedCubeFinder::estimate(
00703     const sensor_msgs::PointCloud2::ConstPtr& input_cloud,
00704     const jsk_recognition_msgs::ParallelEdgeArray::ConstPtr& input_edges)
00705   {
00706      boost::mutex::scoped_lock lock(mutex_);
00707      pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
00708      pcl::PointCloud<PointT>::Ptr all_filtered_cloud (new pcl::PointCloud<PointT>);
00709      pcl::fromROSMsg(*input_cloud, *cloud);
00710      visualization_msgs::Marker marker;
00711      jsk_recognition_msgs::PolygonArray polygon_array;
00712      geometry_msgs::PoseArray pose_array;
00713      jsk_recognition_msgs::BoundingBoxArray box_array;
00714      box_array.header = input_cloud->header;
00715      pose_array.header = input_cloud->header;
00716      std::vector<jsk_recognition_utils::Cube::Ptr> cubes;
00717      std::vector<pcl::PointIndices::Ptr> candidate_cluster_indices;
00718      for (size_t i = 0; i < input_edges->edge_groups.size(); i++) {
00719        jsk_recognition_msgs::ParallelEdge parallel_edge = input_edges->edge_groups[i];
00720        std::vector<pcl::PointIndices::Ptr> edges
00721          = pcl_conversions::convertToPCLPointIndices(
00722            parallel_edge.cluster_indices);
00723        std::vector<pcl::ModelCoefficients::Ptr> coefficients
00724          = pcl_conversions::convertToPCLModelCoefficients(
00725            parallel_edge.coefficients);
00726        std::vector<IndicesCoefficientsTriple> triples
00727          = tripleIndicesAndCoefficients(edges, coefficients);
00728        std::vector<IndicesCoefficientsTriple> perpendicular_triples
00729          = filterPerpendicularEdgeTriples(triples);
00730        if (perpendicular_triples.size() > 0) {
00731          // buildup cube instance...
00732          pcl::PointCloud<PointT>::Ptr points_on_edges(new pcl::PointCloud<PointT>);
00733          pcl_conversions::toPCL(input_cloud->header, points_on_edges->header);
00734          for (size_t j = 0; j < perpendicular_triples.size(); j++) {
00735            pcl::PointCloud<PointT>::Ptr points_on_edge(new pcl::PointCloud<PointT>);
00736            jsk_recognition_utils::Cube::Ptr cube = cubeFromIndicesAndCoefficients(
00737              cloud,
00738              perpendicular_triples[j],
00739              points_on_edge);
00740            *points_on_edges = *points_on_edges + *points_on_edge;
00741            cubes.push_back(cube);
00742          }
00743          pub_debug_filtered_cloud_.publish(points_on_edges);
00744        }
00745      }
00746 
00747      if (cubes.size() > 0) {
00748        for (size_t i = 0; i < cubes.size(); i++) {
00749          // publish cubes
00750          jsk_recognition_msgs::BoundingBox ros_box = cubes[i]->toROSMsg();
00751          ros_box.header = input_cloud->header;
00752          box_array.boxes.push_back(ros_box);
00753          pose_array.poses.push_back(ros_box.pose);
00754        }
00755        pub_.publish(box_array);
00756        pub_pose_array_.publish(pose_array);
00757      }
00758   }
00759 
00760   bool EdgebasedCubeFinder::isPerpendicularVector(
00761     const Eigen::Vector3f& a,
00762     const Eigen::Vector3f& b)
00763   {
00764     double dot = a.normalized().dot(b.normalized());
00765     if (fabs(dot) >= 1.0) {
00766       return false;
00767     }
00768     else {
00769       double theta = fabs(acos(dot));
00770       NODELET_INFO("theta: %f", pcl::rad2deg(theta));
00771       if (fabs(theta - M_PI / 2.0) < pcl::deg2rad(20.0)) {
00772         return true;
00773       }
00774       else {
00775         return false;
00776       }
00777     }
00778   }
00779   
00780   EdgebasedCubeFinder::EdgeRelation EdgebasedCubeFinder::perpendicularEdgeTriple(
00781     const jsk_recognition_utils::Line& edge_a,
00782     const jsk_recognition_utils::Line& edge_b,
00783     const jsk_recognition_utils::Line& edge_c)
00784   {
00785     Eigen::Vector3f a_b_normal, a_c_normal;
00786     edge_a.parallelLineNormal(edge_b, a_b_normal);
00787     edge_a.parallelLineNormal(edge_c, a_c_normal);
00788     if (isPerpendicularVector(a_b_normal, a_c_normal)) {
00789       return A_PERPENDICULAR;
00790     }
00791     else {
00792       Eigen::Vector3f b_a_normal, b_c_normal;
00793       edge_b.parallelLineNormal(edge_a, b_a_normal);
00794       edge_b.parallelLineNormal(edge_c, b_c_normal);
00795       if (isPerpendicularVector(b_a_normal, b_c_normal)) {
00796         return B_PERPENDICULAR;
00797       }
00798       else {
00799         Eigen::Vector3f c_a_normal, c_b_normal;
00800         edge_c.parallelLineNormal(edge_a, c_a_normal);
00801         edge_c.parallelLineNormal(edge_b, c_b_normal);
00802         if (isPerpendicularVector(c_a_normal, c_b_normal)) {
00803           return C_PERPENDICULAR;
00804         }
00805         else {
00806           return NOT_PERPENDICULAR;
00807         }
00808       }
00809     }
00810   }
00811   
00812   std::vector<IndicesCoefficientsTriple>
00813   EdgebasedCubeFinder::filterPerpendicularEdgeTriples(
00814     const std::vector<IndicesCoefficientsTriple>& triples)
00815   {
00816     std::vector<IndicesCoefficientsTriple> ret;
00817     for (size_t i = 0; i < triples.size(); i++) {
00818       pcl::ModelCoefficients::Ptr a_coefficients
00819         = triples[i].get<1>().get<0>();
00820       pcl::ModelCoefficients::Ptr b_coefficients
00821         = triples[i].get<1>().get<1>();
00822       pcl::ModelCoefficients::Ptr c_coefficients
00823         = triples[i].get<1>().get<2>();
00824       jsk_recognition_utils::Line::Ptr edge_a
00825         = jsk_recognition_utils::Line::fromCoefficients(a_coefficients->values);
00826       jsk_recognition_utils::Line::Ptr edge_b
00827         = jsk_recognition_utils::Line::fromCoefficients(b_coefficients->values);
00828       jsk_recognition_utils::Line::Ptr edge_c
00829         = jsk_recognition_utils::Line::fromCoefficients(c_coefficients->values);
00830       // check if these three are perpendicular or not
00831       EdgeRelation relation = perpendicularEdgeTriple(*edge_a,
00832                                                       *edge_b,
00833                                                       *edge_c);
00834       if (relation != NOT_PERPENDICULAR) {
00835         // rearrange
00836         if (relation == A_PERPENDICULAR) {
00837           ret.push_back(triples[i]);
00838         }
00839         else if (relation == B_PERPENDICULAR) {
00840           IndicesCoefficientsTriple new_triple
00841             = boost::make_tuple(
00842               boost::make_tuple(triples[i].get<0>().get<1>(),
00843                                 triples[i].get<0>().get<0>(),
00844                                 triples[i].get<0>().get<2>()),
00845               boost::make_tuple(
00846                           triples[i].get<1>().get<1>(),
00847                           triples[i].get<1>().get<0>(),
00848                           triples[i].get<1>().get<2>()));
00849           ret.push_back(new_triple);
00850         }
00851         else if (relation == C_PERPENDICULAR) {
00852           IndicesCoefficientsTriple new_triple
00853             = boost::make_tuple(
00854               boost::make_tuple(triples[i].get<0>().get<2>(),
00855                                 triples[i].get<0>().get<0>(),
00856                                 triples[i].get<0>().get<1>()),
00857               boost::make_tuple(
00858                           triples[i].get<1>().get<2>(),
00859                           triples[i].get<1>().get<0>(),
00860                           triples[i].get<1>().get<1>()));
00861           ret.push_back(new_triple);
00862         }
00863       }
00864     }
00865     return ret;
00866   }
00867   
00868   void EdgebasedCubeFinder::estimate2(
00869     const sensor_msgs::PointCloud2::ConstPtr& input_cloud,
00870     const jsk_recognition_msgs::ParallelEdgeArray::ConstPtr& input_edges)
00871   {
00872      boost::mutex::scoped_lock lock(mutex_);
00873      pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
00874      pcl::PointCloud<PointT>::Ptr all_filtered_cloud (new pcl::PointCloud<PointT>);
00875      pcl::fromROSMsg(*input_cloud, *cloud);
00876      visualization_msgs::Marker marker;
00877      jsk_recognition_msgs::PolygonArray polygon_array;
00878      std::vector<pcl::PointIndices::Ptr> candidate_cluster_indices;
00879      
00880      polygon_array.header = input_cloud->header;
00881      marker.pose.orientation.w = 1.0;
00882      marker.color.a = 1.0;
00883      marker.color.r = 1.0;
00884      marker.header = input_cloud->header;
00885      marker.scale.x = 0.01;
00886      marker.type = visualization_msgs::Marker::LINE_LIST;
00887      NODELET_INFO("%lu parallel edge groups", input_edges->edge_groups.size());
00888      geometry_msgs::PoseArray pose_array;
00889      pose_array.header = input_cloud->header;
00890      jsk_recognition_msgs::BoundingBoxArray ros_output;
00891      ros_output.header = input_cloud->header;
00892 
00893      for (size_t i = 0; i < input_edges->edge_groups.size(); i++) {
00894        jsk_recognition_msgs::ParallelEdge parallel_edge = input_edges->edge_groups[i];
00895 
00896        if (parallel_edge.cluster_indices.size() <= 1) {
00897          NODELET_ERROR("parallel edge group has only %lu edges",
00898                        parallel_edge.cluster_indices.size());
00899          continue;
00900        }
00901        NODELET_INFO("%lu parallel edge groups has %lu edges",
00902                     i, parallel_edge.cluster_indices.size());
00904        // first convert all the pcl_msgs/PointIndices to pcl::PointIndices
00906        std::vector<pcl::PointIndices::Ptr> edges
00907          = pcl_conversions::convertToPCLPointIndices(
00908            parallel_edge.cluster_indices);
00909        std::vector<pcl::ModelCoefficients::Ptr> coefficients
00910          = pcl_conversions::convertToPCLModelCoefficients(
00911            parallel_edge.coefficients);
00912 
00913        std::vector<IndicesPair> pairs = combinateIndices(edges);
00914        std::vector<CoefficientsPair> coefficients_pair
00915          = combinateCoefficients(coefficients);
00916        std::vector<IndicesPair> filtered_indices_pairs;
00917        std::vector<CoefficientsPair> filtered_coefficients_pairs;
00918 
00919        filtered_indices_pairs = pairs;
00920        filtered_coefficients_pairs = coefficients_pair;
00921        // filterPairsBasedOnParallelEdgeDistances(
00922        //   pairs, coefficients_pair,
00923        //   filtered_indices_pairs, filtered_coefficients_pairs);
00924 
00925        // convex based filtering...
00926        std::vector<jsk_recognition_utils::ConvexPolygon::Ptr> convexes;
00927        for (size_t j = 0; j < filtered_coefficients_pairs.size(); j++) {
00928          jsk_recognition_utils::ConvexPolygon::Ptr convex
00929            = convexFromPairs(cloud, filtered_coefficients_pairs[j],
00930                              pairs[j]);
00931          convexes.push_back(convex);
00932        }
00933        std::vector<int> filtered_indices;
00934        filterBasedOnConvex(cloud, convexes, filtered_indices);
00935 
00936        pcl::PointIndices::Ptr filtered_cube_candidate_indices(new pcl::PointIndices);
00937        for (size_t j = 0; j < filtered_indices.size(); j++) {
00938          int index = filtered_indices[j];
00939          jsk_recognition_utils::ConvexPolygon::Ptr target_convex = convexes[index];
00940          IndicesPair target_edge_indices_pair
00941            = filtered_indices_pairs[index];
00942          CoefficientsPair target_edge_coefficients_pair
00943            = filtered_coefficients_pairs[index];
00944          // 1. roughly segment the points around the plane
00945          //pcl::PointCloud<PointT>::Ptr filtered_cloud
00946          pcl::PointIndices::Ptr filtered_indices
00947            = preparePointCloudForRANSAC(
00948              target_convex, target_edge_coefficients_pair, cloud);
00949          // *filtered_cube_candidate_indices
00950          //   = *filtered_indices + *filtered_cube_candidate_indices;
00951          filtered_cube_candidate_indices
00952            = jsk_recognition_utils::addIndices(*filtered_cube_candidate_indices,
00953                         *filtered_indices);       
00954            
00955          // ROS_INFO("%lu -> %lu", cloud->points.size(), filtered_cloud->points.size());
00956          // *all_filtered_cloud = *all_filtered_cloud + *filtered_cloud;
00957          // // 2. estimate a plane parallel to the convex using RANSAC
00958          // pcl::ModelCoefficients::Ptr
00959          //   parallel_plane_coefficients (new pcl::ModelCoefficients);
00960          // pcl::PointIndices::Ptr
00961          //   parallel_plane_inliers (new pcl::PointIndices);
00962 
00963          // estimateParallelPlane(target_convex, filtered_cloud,
00964          //                       parallel_plane_inliers,
00965          //                       parallel_plane_coefficients);
00966          // if (parallel_plane_inliers->indices.size() > 0) {
00967          //   jsk_recognition_utils::ConvexPolygon::Ptr parallel_convex
00968          //     = jsk_recognition_utils::convexFromCoefficientsAndInliers<PointT>(
00969          //       filtered_cloud,
00970          //       parallel_plane_inliers,
00971          //       parallel_plane_coefficients);
00972          //   if (parallel_convex) {
00973          //     pcl::ModelCoefficients::Ptr
00974          //       perpendicular_plane_coefficients (new pcl::ModelCoefficients);
00975          //     pcl::PointIndices::Ptr
00976          //       perpendicular_plane_inliers (new pcl::PointIndices);
00977          //     estimatePerpendicularPlane(parallel_convex,
00978          //                                target_edge_coefficients_pair,
00979          //                                filtered_cloud,
00980          //                                perpendicular_plane_inliers,
00981          //                                perpendicular_plane_coefficients);
00982          //     if (perpendicular_plane_inliers->indices.size() > 0) {
00983          //      jsk_recognition_utils::ConvexPolygon::Ptr perpendicular_convex
00984          //        = jsk_recognition_utils::convexFromCoefficientsAndInliers<PointT>(
00985          //          filtered_cloud,
00986          //          perpendicular_plane_inliers,
00987          //          perpendicular_plane_coefficients);
00988          //      if (perpendicular_convex) {
00989          //        if (perpendicular_convex->angle(*parallel_convex) < pcl::deg2rad(10.0)) {
00990          //          geometry_msgs::PolygonStamped ros_polygon;
00991          //          ros_polygon.header = input_cloud->header;
00992          //          ros_polygon.polygon = parallel_convex->toROSMsg();
00993          //          polygon_array.polygons.push_back(ros_polygon);
00994          //          geometry_msgs::PolygonStamped ros_polygon2;
00995          //          ros_polygon2.header = input_cloud->header;
00996          //          ros_polygon2.polygon = perpendicular_convex->toROSMsg();
00997          //          polygon_array.polygons.push_back(ros_polygon2);
00998          //        }
00999          //      }
01000          //    }
01001          //   }
01002          // }
01003         // if (parallel_plane) {
01004         //   // 3. estimate a plane perpendicular to the convex using RANSAC
01005         //   pcl::ModelCoefficients::Ptr perpendicular_plane
01006         //     = estimatePerpendicularPlane(convex, filtered_cloud);
01007         //   if (perpendicular_plane) {
01008         //     // success to estimate, it's cube
01009         //     Cube::Ptr cube
01010         //       = makeupCubeResult();
01011         //     cubes.push_back(cube);
01012         //   }
01013         //   else {
01014         //     // failed to estimate perpendicular plane
01015         //     ROS_INFO("failed to estimate perpendicular plane");
01016         //   }
01017         // }
01018         // else {
01019         //   // failed to estimate parallel plane
01020         //   ROS_INFO("failed to estimate parallel plane");
01021         // }
01022 //      }
01023       
01024       // for (size_t j = 0; j < filtered_indices.size(); j++) {
01025       //   int pair_index = filtered_indices[j];
01026       //   jsk_recognition_utils::ConvexPolygon::Ptr convex = convexes[filtered_indices[j]];
01027         
01028       //   Vertices vs = convex->getVertices();
01029       //   for (size_t k = 0; k < vs.size(); k++) {
01030       //     Eigen::Vector3f A, B;
01031       //     Eigen::Vector3d Ad, Bd;
01032       //     A = vs[k];
01033       //     if (k + 1 != vs.size()) {
01034       //       B = vs[k + 1];
01035       //     }
01036       //     else {
01037       //       B = vs[0];
01038       //     }
01039       //     geometry_msgs::Point AP, BP;
01040       //     convertEigenVector(A, Ad);
01041       //     convertEigenVector(B, Bd);
01042       //     tf::pointEigenToMsg(Ad, AP);
01043       //     tf::pointEigenToMsg(Bd, BP);
01044       //     marker.points.push_back(AP);
01045       //     marker.points.push_back(BP);
01046       //     std_msgs::ColorRGBA green;
01047       //     green.a = 1.0; green.g = 1.0;
01048       //     marker.colors.push_back(green);
01049       //     marker.colors.push_back(green);
01050       //   }
01051        }
01052        candidate_cluster_indices.push_back(filtered_cube_candidate_indices);
01053 
01054        // estimate cube
01055        pcl::PointIndices::Ptr first_inliers(new pcl::PointIndices);
01056        pcl::ModelCoefficients::Ptr first_coefficients(new pcl::ModelCoefficients);
01057        jsk_recognition_utils::ConvexPolygon::Ptr first_polygon
01058          = estimateConvexPolygon(
01059            cloud,
01060            filtered_cube_candidate_indices,
01061            first_coefficients,
01062            first_inliers);
01063        if (first_polygon) {
01064          geometry_msgs::PolygonStamped first_polygon_ros;
01065          first_polygon_ros.polygon = first_polygon->toROSMsg();
01066          first_polygon_ros.header = input_cloud->header;
01067          polygon_array.polygons.push_back(first_polygon_ros);
01068        }
01069       // std::vector<CubeHypothesis::Ptr> hypothesis_list;
01070       // NODELET_INFO("building hypothesis");
01071       // NODELET_INFO("pair size: %lu", pairs.size());
01072       // for (size_t j = 0; j < pairs.size(); j++) {
01073       //   NODELET_INFO("j -> %lu", j);
01074       //   IndicesPair pair = pairs[j];
01075       //   CoefficientsPair cpair = coefficients_pair[j];
01076       //   PlanarCubeHypothesis::Ptr
01077       //     planar_hypothesis (new PlanarCubeHypothesis(
01078       //                          pair, cpair, outlier_threshold_));
01079       //   DiagnoalCubeHypothesis::Ptr
01080       //     diagnoal_hypothesis (new DiagnoalCubeHypothesis(
01081       //                            pair, cpair, outlier_threshold_));
01082       //   //hypothesis_list.push_back(planar_hypothesis);
01083       //   hypothesis_list.push_back(diagnoal_hypothesis);
01084       // }
01085       // NODELET_INFO("estimating hypothesis");
01086       // for (size_t j = 0; j < hypothesis_list.size(); j++) {
01087       //   hypothesis_list[j]->estimate(*cloud);
01088       // }
01089       // // find max evaluated
01090       // NODELET_INFO("find max hypothesis");
01091       // CubeHypothesis::Ptr max_hypothesis;
01092       // double max_eval = -DBL_MAX;
01093       // for (size_t i = 0; i < hypothesis_list.size(); i++) {
01094       //   if (max_eval < hypothesis_list[i]->getValue()) {
01095       //     max_eval = hypothesis_list[i]->getValue();
01096       //     max_hypothesis = hypothesis_list[i];
01097       //   }
01098       // }
01099       // NODELET_INFO("convert to ros msg");
01100       // BoundingBox msg = max_hypothesis->getCube()->toROSMsg();
01101       // msg.header = input_cloud->header;
01102       // ros_output.boxes.push_back(msg);
01103       // pose_array.poses.push_back(msg.pose);
01104     }
01105     pub_.publish(ros_output);
01106     pub_pose_array_.publish(pose_array);
01107     pub_debug_marker_.publish(marker);
01108     sensor_msgs::PointCloud2 ros_cloud;
01109     pcl::toROSMsg(*all_filtered_cloud, ros_cloud);
01110     ros_cloud.header = input_cloud->header;
01111     pub_debug_filtered_cloud_.publish(ros_cloud);
01112     pub_debug_polygons_.publish(polygon_array);
01113 
01114     // convert std::vector<pcl::PointIndices::Ptr> to ClusterPointIndices
01115     jsk_recognition_msgs::ClusterPointIndices ros_cluster_indices;
01116     ros_cluster_indices.header = input_cloud->header;
01117     for (size_t i = 0; i < candidate_cluster_indices.size(); i++) {
01118       PCLIndicesMsg indices_msg;
01119       indices_msg.header = input_cloud->header;
01120       indices_msg.indices = candidate_cluster_indices[i]->indices;
01121       ros_cluster_indices.cluster_indices.push_back(indices_msg);
01122     }
01123     pub_debug_clusers_.publish(ros_cluster_indices);
01124   }
01125 
01126   std::vector<IndicesCoefficientsTriple>
01127   EdgebasedCubeFinder::tripleIndicesAndCoefficients(
01128     const std::vector<pcl::PointIndices::Ptr>& indices,
01129     const std::vector<pcl::ModelCoefficients::Ptr>& coefficients)
01130   {
01131     if (indices.size() != coefficients.size()) {
01132       NODELET_ERROR("size of indices and coefficients are not same");
01133       return std::vector<IndicesCoefficientsTriple>();
01134     }
01135 
01136     if (indices.size() <= 2 && coefficients.size() <= 2) {
01137       NODELET_WARN("[EdgebasedCubeFinder::tripleIndicesAndCoefficients] no enough canddiates");
01138       return std::vector<IndicesCoefficientsTriple>();
01139     }
01140     std::vector<IndicesCoefficientsTriple> ret;
01141     for (size_t i = 0; i < indices.size() - 2; i++) {
01142       for (size_t j = i + 1; j < indices.size() - 1; j++) {
01143         for (size_t k = j + 1; k < indices.size(); k++) {
01144           IndicesTriple indices_triple
01145             = boost::make_tuple(indices[i],
01146                                 indices[j],
01147                                 indices[k]);
01148           CoefficientsTriple coefficients_triple
01149             = boost::make_tuple(coefficients[i],
01150                                 coefficients[j],
01151                                 coefficients[k]);
01152           IndicesCoefficientsTriple indices_coefficients_triple
01153             = boost::make_tuple(indices_triple,
01154                                 coefficients_triple);
01155           ret.push_back(indices_coefficients_triple);
01156         }
01157       }
01158     }
01159     return ret;
01160   }
01161   
01162   jsk_recognition_utils::ConvexPolygon::Ptr EdgebasedCubeFinder::estimateConvexPolygon(
01163     const pcl::PointCloud<PointT>::Ptr cloud,
01164     const pcl::PointIndices::Ptr indices,
01165     pcl::ModelCoefficients::Ptr coefficients,
01166     pcl::PointIndices::Ptr inliers)
01167   {
01169     // RANSAC
01171     pcl::SACSegmentation<PointT> seg;
01172     seg.setOptimizeCoefficients (true);
01173     seg.setModelType (pcl::SACMODEL_PLANE);
01174     seg.setMethodType (pcl::SAC_RANSAC);
01175     seg.setInputCloud(cloud);
01176     seg.setIndices(indices);
01177     seg.setDistanceThreshold(0.003);
01178     seg.segment(*inliers, *coefficients);
01180     // project points to the plane
01182     if (inliers->indices.size() > 0) {
01183       return jsk_recognition_utils::convexFromCoefficientsAndInliers<PointT>(
01184         cloud, inliers, coefficients);
01185     }
01186     else {
01187       return jsk_recognition_utils::ConvexPolygon::Ptr();
01188     }
01189   }
01190   
01191   std::vector<IndicesPair> EdgebasedCubeFinder::combinateIndices(
01192     const std::vector<pcl::PointIndices::Ptr>& indices)
01193   {
01194     std::vector<IndicesPair> ret;
01195     for(size_t i = 0; i < indices.size() - 1; i++) {
01196       for (size_t j = i + 1; j < indices.size(); j++) {
01197         IndicesPair pair = boost::make_tuple(indices[i],
01198                                              indices[j]);
01199         ret.push_back(pair);
01200       }
01201     }
01202     return ret;
01203   }
01204 
01205   std::vector<CoefficientsPair> EdgebasedCubeFinder::combinateCoefficients(
01206     const std::vector<pcl::ModelCoefficients::Ptr>& coefficients)
01207   {
01208     std::vector<CoefficientsPair> ret;
01209     for(size_t i = 0; i < coefficients.size() - 1; i++) {
01210       for (size_t j = i + 1; j < coefficients.size(); j++) {
01211         CoefficientsPair pair = boost::make_tuple(coefficients[i],
01212                                                   coefficients[j]);
01213         ret.push_back(pair);
01214       }
01215     }
01216     return ret;
01217   }
01218 }
01219 
01220 #include <pluginlib/class_list_macros.h>
01221 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::EdgebasedCubeFinder, nodelet::Nodelet);
01222 


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