00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
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_pcl_ros/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
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 pointFromVectorToVector<Eigen::Vector4f, Eigen::Vector3f>(centroid, output);
00084 }
00085
00086 void CubeHypothesis::getLinePoints(
00087 const Line& line,
00088 const pcl::PointCloud<pcl::PointXYZRGB>& cloud,
00089 const pcl::PointIndices::Ptr indices,
00090 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 ConvexPolygon::Ptr CubeHypothesis::buildConvexPolygon(
00108 const PointPair& a_edge_pair, const PointPair& b_edge_pair)
00109 {
00110 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 ConvexPolygon::Ptr convex (new ConvexPolygon(vertices));
00116 return convex;
00117 }
00118
00119 double CubeHypothesis::evaluatePointOnPlanes(
00120 const pcl::PointCloud<pcl::PointXYZRGB>& cloud,
00121 ConvexPolygon& polygon_a,
00122 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)) {
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
00140 return a_indices.size() + b_indices.size();
00141 }
00142
00143 PointPair CubeHypothesis::computeAxisEndPoints(
00144 const Line& axis,
00145 const PointPair& a_candidates,
00146 const PointPair& b_candidates)
00147 {
00148 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 JSK_ROS_INFO("[foot_point] [%f, %f, %f]", p[0], p[1], p[2]);
00156 }
00157
00158 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 JSK_ROS_INFO("min_alpha_point: [%f, %f, %f]", min_alpha_point[0], min_alpha_point[1], min_alpha_point[2]);
00180 JSK_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 Line::Ptr line_a
00205 = Line::fromCoefficients(coefficients_pair_.get<0>()->values);
00206 Line::Ptr line_b
00207 = 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 Line::Ptr axis = line_a->midLine(*line_b);
00215 Eigen::Vector3f center;
00216 axis->getOrigin(center);
00217 JSK_ROS_INFO("line_a:");
00218 line_a->print();
00219 JSK_ROS_INFO("line_b:");
00220 line_b->print();
00221 JSK_ROS_INFO("axis:");
00222 axis->print();
00223 JSK_ROS_INFO("r: %f", r);
00224
00225
00226
00227
00228
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 JSK_ROS_INFO("centroid_a: [%f, %f, %f]", centroid_a[0], centroid_a[1], centroid_a[2]);
00234 JSK_ROS_INFO("centroid_b: [%f, %f, %f]", centroid_b[0], centroid_b[1], centroid_b[2]);
00235 Line::Ptr line_a_aligned = axis->parallelLineOnAPoint(centroid_a);
00236 Line::Ptr line_b_aligned = axis->parallelLineOnAPoint(centroid_b);
00237 JSK_ROS_INFO("line_a_aligned:");
00238 line_a_aligned->print();
00239 JSK_ROS_INFO("line_b_aligned:");
00240 line_b_aligned->print();
00241
00242 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 PointPair line_a_end_points = line_a->findEndPoints(line_a_points);
00248 PointPair line_b_end_points = line_b->findEndPoints(line_b_points);
00249 double max_v = - DBL_MAX;
00250 double max_theta;
00251 Line::Ptr max_line_c;
00252 PointPair max_line_c_a_points, max_line_c_b_points;
00253 for (size_t i = 0; i < resolution_; i++) {
00254 JSK_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
00259
00260
00261
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
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 Line::Ptr line_c = axis->parallelLineOnAPoint(point_on_y);
00273
00274
00275
00276
00277
00278
00279
00280
00281
00282
00283
00284
00285
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 PointPair line_c_a_end_points = boost::make_tuple(line_c_a_min_point,
00293 line_c_a_max_point);
00294 PointPair line_c_b_end_points = boost::make_tuple(line_c_b_min_point,
00295 line_c_b_max_point);
00296 ConvexPolygon::Ptr plane_a = buildConvexPolygon(line_a_end_points,
00297 line_c_a_end_points);
00298 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
00311 PointPair axis_end_points = computeAxisEndPoints(
00312 *axis,
00313 max_line_c_a_points,
00314 max_line_c_b_points);
00315 JSK_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 JSK_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
00321 JSK_ROS_INFO("midpoint: [%f, %f, %f]", midpoint[0], midpoint[1], midpoint[2]);
00322 cube_.reset(new 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 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 (!isnan(p.x) && !isnan(p.y) && !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<ConvexPolygon::Ptr>& convexes,
00348 std::vector<int>& output_indices)
00349 {
00350
00351 for (size_t i = 0; i < convexes.size(); i++) {
00352 ConvexPolygon::Ptr convex = convexes[i];
00353 if (true) {
00354
00355
00356
00357
00358
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
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
00404 void EdgebasedCubeFinder::subscribe()
00405 {
00407
00409 sub_input_.subscribe(*pnh_, "input", 1);
00410 sub_edges_.subscribe(*pnh_, "input_edges", 1);
00411 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
00412 sync_->connectInput(sub_input_, sub_edges_);
00413 sync_->registerCallback(boost::bind(
00414 &EdgebasedCubeFinder::estimate, this, _1, _2));
00415 }
00416
00417 void EdgebasedCubeFinder::unsubscribe()
00418 {
00419 sub_input_.unsubscribe();
00420 sub_edges_.unsubscribe();
00421 }
00422
00423 Line::Ptr EdgebasedCubeFinder::midLineFromCoefficientsPair(
00424 const CoefficientsPair& pair)
00425 {
00426 pcl::ModelCoefficients::Ptr coefficients_a = pair.get<0>();
00427 pcl::ModelCoefficients::Ptr coefficients_b = pair.get<1>();
00428 Line::Ptr line_a = Line::fromCoefficients(coefficients_a->values);
00429 Line::Ptr line_b = Line::fromCoefficients(coefficients_b->values);
00430 return line_a->midLine(*line_b);
00431 }
00432
00433 pcl::PointCloud<EdgebasedCubeFinder::PointT>::Ptr EdgebasedCubeFinder::extractPointCloud(
00434 const pcl::PointCloud<PointT>::Ptr cloud,
00435 const pcl::PointIndices::Ptr indices)
00436 {
00437 pcl::PointCloud<PointT>::Ptr ret (new pcl::PointCloud<PointT>);
00438 pcl::ExtractIndices<PointT> ex;
00439 ex.setInputCloud(cloud);
00440 ex.setIndices(indices);
00441 ex.filter(*ret);
00442 return ret;
00443 }
00444
00445 PointPair EdgebasedCubeFinder::minMaxPointOnLine(
00446 const Line& line,
00447 const pcl::PointCloud<PointT>::Ptr cloud)
00448 {
00449 Vertices points;
00450 for (size_t i = 0; i < cloud->points.size(); i++) {
00451 PointT p = cloud->points[i];
00452 Eigen::Vector3f eigen_p = p.getVector3fMap();
00453 Eigen::Vector3f foot;
00454 line.foot(eigen_p, foot);
00455 points.push_back(foot);
00456 }
00457 return line.findEndPoints(points);
00458 }
00459
00460 ConvexPolygon::Ptr EdgebasedCubeFinder::convexFromPairs(
00461 const pcl::PointCloud<PointT>::Ptr cloud,
00462 const CoefficientsPair& coefficients_pair,
00463 const IndicesPair& indices_pair)
00464 {
00465 pcl::ModelCoefficients::Ptr coefficients_a = coefficients_pair.get<0>();
00466 pcl::ModelCoefficients::Ptr coefficients_b = coefficients_pair.get<1>();
00467 pcl::PointIndices::Ptr indices_a = indices_pair.get<0>();
00468 pcl::PointIndices::Ptr indices_b = indices_pair.get<1>();
00469
00470 pcl::PointCloud<PointT>::Ptr cloud_a = extractPointCloud(cloud, indices_a);
00471 pcl::PointCloud<PointT>::Ptr cloud_b = extractPointCloud(cloud, indices_b);
00472
00473 Line::Ptr line_a = Line::fromCoefficients(coefficients_a->values);
00474 Line::Ptr line_b = Line::fromCoefficients(coefficients_b->values);
00475 PointPair a_min_max = minMaxPointOnLine(*line_a, cloud_a);
00476 PointPair b_min_max = minMaxPointOnLine(*line_b, cloud_b);
00477 Vertices vertices;
00478 vertices.push_back(a_min_max.get<0>());
00479 vertices.push_back(a_min_max.get<1>());
00480 vertices.push_back(b_min_max.get<1>());
00481 vertices.push_back(b_min_max.get<0>());
00482 ConvexPolygon::Ptr ret (new ConvexPolygon(vertices));
00483 return ret;
00484 }
00485
00486 void EdgebasedCubeFinder::filterPairsBasedOnParallelEdgeDistances(
00487 const std::vector<IndicesPair>& pairs,
00488 const std::vector<CoefficientsPair>& coefficients_pair,
00489 std::vector<IndicesPair>& filtered_indices_pairs,
00490 std::vector<CoefficientsPair>& filtered_coefficients_pairs)
00491 {
00492 for (size_t i = 0; i < coefficients_pair.size(); i++) {
00493 CoefficientsPair coefficients = coefficients_pair[i];
00494 pcl::ModelCoefficients::Ptr coefficients_a = coefficients_pair[i].get<0>();
00495 pcl::ModelCoefficients::Ptr coefficients_b = coefficients_pair[i].get<1>();
00496 Line::Ptr line_a = Line::fromCoefficients(coefficients_a->values);
00497 Line::Ptr line_b = Line::fromCoefficients(coefficients_b->values);
00498
00499
00500 Line::Ptr axis = line_a->midLine(*line_b);
00501 Eigen::Vector3f origin_a, origin_b;
00502 line_a->getOrigin(origin_a);
00503 line_b->getOrigin(origin_b);
00504 Line::Ptr line_a_aligned = axis->parallelLineOnAPoint(origin_a);
00505 Line::Ptr line_b_aligned = axis->parallelLineOnAPoint(origin_b);
00506 Eigen::Vector3f distance_vector;
00507 line_a_aligned->parallelLineNormal(*line_b_aligned, distance_vector);
00508 double distance = distance_vector.norm();
00509
00510 JSK_ROS_INFO("d: %f", distance);
00511 if (distance < parallel_edge_distance_max_threshold_ &&
00512 distance > parallel_edge_distance_min_threshold_) {
00513 filtered_indices_pairs.push_back(pairs[i]);
00514 filtered_coefficients_pairs.push_back(coefficients);
00515 }
00516 }
00517 }
00518
00519
00520 pcl::PointIndices::Ptr
00521 EdgebasedCubeFinder::preparePointCloudForRANSAC(
00522 const ConvexPolygon::Ptr convex,
00523 const CoefficientsPair& edge_coefficients_pair,
00524 const pcl::PointCloud<PointT>::Ptr cloud)
00525 {
00526
00527 pcl::PointIndices::Ptr indices(new pcl::PointIndices);
00528 ConvexPolygon::Ptr magnified_convex = convex->magnify(1.1);
00529 pcl::PointCloud<PointT>::Ptr ret (new pcl::PointCloud<PointT>);
00530 for (size_t i = 0; i < cloud->points.size(); i++) {
00531 PointT p = cloud->points[i];
00532 if (!isnan(p.x) && !isnan(p.y) && !isnan(p.z)) {
00533 Eigen::Vector3f ep = p.getVector3fMap();
00534 Eigen::Vector3f foot;
00535 magnified_convex->projectOnPlane(ep, foot);
00536 if (magnified_convex->isInside(foot) && convex->distanceSmallerThan(ep, outlier_threshold_)) {
00537
00538
00539 indices->indices.push_back(i);
00540 }
00541 }
00542 }
00543 return indices;
00544 }
00545
00546 void EdgebasedCubeFinder::estimateParallelPlane(
00547 const ConvexPolygon::Ptr convex,
00548 const pcl::PointCloud<PointT>::Ptr filtered_cloud,
00549 pcl::PointIndices::Ptr output_inliers,
00550 pcl::ModelCoefficients::Ptr output_coefficients)
00551 {
00552 Eigen::Vector3f normal = convex->getNormal();
00553 pcl::SACSegmentation<PointT> seg;
00554 seg.setOptimizeCoefficients (true);
00555 seg.setModelType (pcl::SACMODEL_PERPENDICULAR_PLANE);
00556 seg.setMethodType (pcl::SAC_RANSAC);
00557 seg.setDistanceThreshold (outlier_threshold_);
00558 seg.setInputCloud(filtered_cloud);
00559 seg.setMaxIterations (10000);
00560 seg.setAxis(normal);
00561 seg.setEpsAngle(pcl::deg2rad(10.0));
00562 seg.segment (*output_inliers, *output_coefficients);
00563 }
00564
00565 void EdgebasedCubeFinder::estimatePerpendicularPlane(
00566 const ConvexPolygon::Ptr convex,
00567 const CoefficientsPair& edge_coefficients,
00568 const pcl::PointCloud<PointT>::Ptr filtered_cloud,
00569 pcl::PointIndices::Ptr output_inliers,
00570 pcl::ModelCoefficients::Ptr output_coefficients)
00571 {
00572 Eigen::Vector3f normal_a = convex->getNormal();
00573 Line::Ptr mid_line = midLineFromCoefficientsPair(edge_coefficients);
00574 Eigen::Vector3f normal_b;
00575 mid_line->getDirection(normal_b);
00576 Eigen::Vector3f normal = normal_a.cross(normal_b);
00577 pcl::SACSegmentation<PointT> seg;
00578 seg.setOptimizeCoefficients (true);
00579 seg.setModelType (pcl::SACMODEL_PERPENDICULAR_PLANE);
00580 seg.setMethodType (pcl::SAC_RANSAC);
00581 seg.setDistanceThreshold (outlier_threshold_);
00582 seg.setInputCloud(filtered_cloud);
00583 seg.setMaxIterations (10000);
00584 seg.setAxis(normal);
00585 seg.setEpsAngle(pcl::deg2rad(5.0));
00586 seg.segment (*output_inliers, *output_coefficients);
00587 }
00588
00589 Cube::Ptr EdgebasedCubeFinder::cubeFromIndicesAndCoefficients(
00590 const pcl::PointCloud<EdgebasedCubeFinder::PointT>::Ptr cloud,
00591 const IndicesCoefficientsTriple& indices_coefficients_triple,
00592 pcl::PointCloud<EdgebasedCubeFinder::PointT>::Ptr points_on_edge)
00593 {
00594 Eigen::Vector3f ex, ey, ez;
00595 CoefficientsTriple coefficients_triple
00596 = indices_coefficients_triple.get<1>();
00597 IndicesTriple indices_triple
00598 = indices_coefficients_triple.get<0>();
00599
00600 Line::Ptr mid_line
00601 = Line::fromCoefficients(coefficients_triple.get<0>()->values);
00602 Line::Ptr line_a
00603 = Line::fromCoefficients(coefficients_triple.get<1>()->values);
00604 Line::Ptr line_b
00605 = Line::fromCoefficients(coefficients_triple.get<2>()->values);
00606
00607 if (!mid_line->isSameDirection(*line_a)) {
00608 line_a = line_a->flip();
00609 }
00610 if (!mid_line->isSameDirection(*line_b)) {
00611 line_b = line_b->flip();
00612 }
00613 Line::Ptr axis = line_a->midLine(*line_b);
00614
00615 pcl::PointCloud<PointT>::Ptr point_on_a
00616 = extractPointCloud(cloud,
00617 indices_triple.get<1>());
00618 pcl::PointCloud<PointT>::Ptr point_on_b
00619 = extractPointCloud(cloud,
00620 indices_triple.get<2>());
00621 pcl::PointCloud<PointT>::Ptr point_on_c
00622 = extractPointCloud(cloud,
00623 indices_triple.get<0>());
00624 Eigen::Vector4f a_centroid4, b_centroid4, c_centroid4;
00625 Eigen::Vector3f a_centroid, b_centroid, c_centroid;
00626 pcl::compute3DCentroid(*point_on_a, a_centroid4);
00627 pcl::compute3DCentroid(*point_on_b, b_centroid4);
00628 pcl::compute3DCentroid(*point_on_c, c_centroid4);
00629 pointFromVectorToVector<Eigen::Vector4f, Eigen::Vector3f>(
00630 a_centroid4, a_centroid);
00631 pointFromVectorToVector<Eigen::Vector4f, Eigen::Vector3f>(
00632 b_centroid4, b_centroid);
00633 pointFromVectorToVector<Eigen::Vector4f, Eigen::Vector3f>(
00634 c_centroid4, c_centroid);
00635
00636 Line::Ptr line_a_aligned = axis->parallelLineOnAPoint(a_centroid);
00637 Line::Ptr line_b_aligned = axis->parallelLineOnAPoint(b_centroid);
00638 Line::Ptr mid_line_aligned = axis->parallelLineOnAPoint(c_centroid);
00639
00640 pcl::PointCloud<PointT>::Ptr all_points(new pcl::PointCloud<PointT>);
00641 *all_points = *point_on_a + *point_on_b;
00642 *all_points = *all_points + *point_on_c;
00643 *points_on_edge = *all_points;
00644
00645
00646
00647
00648
00649
00650
00651
00652
00653
00654
00655
00656
00657
00658
00659 PointPair min_max_points = minMaxPointOnLine(*axis, all_points);
00660 PointT min_point, max_point;
00661
00662
00663
00664
00665
00666
00667
00668
00669 Eigen::Vector3f center_point
00670 = (min_max_points.get<0>() + min_max_points.get<1>()) / 2.0;
00671 double z_width = (min_max_points.get<0>() - min_max_points.get<1>()).norm();
00672 mid_line_aligned->getDirection(ez);
00673 mid_line_aligned->parallelLineNormal(*line_a_aligned, ex);
00674 mid_line_aligned->parallelLineNormal(*line_b_aligned, ey);
00675
00676 double x_width = ex.norm();
00677 double y_width = ey.norm();
00678
00679 ex.normalize();
00680 ey.normalize();
00681 ez.normalize();
00682
00683 JSK_ROS_INFO("ex: [%f, %f, %f]", ex[0], ex[1], ex[2]);
00684 JSK_ROS_INFO("ey: [%f, %f, %f]", ey[0], ey[1], ey[2]);
00685 JSK_ROS_INFO("ez: [%f, %f, %f]", ez[0], ez[1], ez[2]);
00686
00687 if (ex.cross(ey).dot(ez) < 0) {
00688 ez = -ez;
00689 }
00690
00691 Eigen::Quaternionf rot = rotFrom3Axis(ex, ey, ez);
00692 std::vector<double> dimensions;
00693 dimensions.push_back(x_width);
00694 dimensions.push_back(y_width);
00695 dimensions.push_back(z_width);
00696 Cube::Ptr ret (new Cube(center_point, rot, dimensions));
00697 return ret;
00698 }
00699
00700 void EdgebasedCubeFinder::estimate(
00701 const sensor_msgs::PointCloud2::ConstPtr& input_cloud,
00702 const jsk_recognition_msgs::ParallelEdgeArray::ConstPtr& input_edges)
00703 {
00704 boost::mutex::scoped_lock lock(mutex_);
00705 pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
00706 pcl::PointCloud<PointT>::Ptr all_filtered_cloud (new pcl::PointCloud<PointT>);
00707 pcl::fromROSMsg(*input_cloud, *cloud);
00708 visualization_msgs::Marker marker;
00709 jsk_recognition_msgs::PolygonArray polygon_array;
00710 geometry_msgs::PoseArray pose_array;
00711 jsk_recognition_msgs::BoundingBoxArray box_array;
00712 box_array.header = input_cloud->header;
00713 pose_array.header = input_cloud->header;
00714 std::vector<Cube::Ptr> cubes;
00715 std::vector<pcl::PointIndices::Ptr> candidate_cluster_indices;
00716 for (size_t i = 0; i < input_edges->edge_groups.size(); i++) {
00717 jsk_recognition_msgs::ParallelEdge parallel_edge = input_edges->edge_groups[i];
00718 std::vector<pcl::PointIndices::Ptr> edges
00719 = pcl_conversions::convertToPCLPointIndices(
00720 parallel_edge.cluster_indices);
00721 std::vector<pcl::ModelCoefficients::Ptr> coefficients
00722 = pcl_conversions::convertToPCLModelCoefficients(
00723 parallel_edge.coefficients);
00724 std::vector<IndicesCoefficientsTriple> triples
00725 = tripleIndicesAndCoefficients(edges, coefficients);
00726 std::vector<IndicesCoefficientsTriple> perpendicular_triples
00727 = filterPerpendicularEdgeTriples(triples);
00728 if (perpendicular_triples.size() > 0) {
00729
00730 pcl::PointCloud<PointT>::Ptr points_on_edges(new pcl::PointCloud<PointT>);
00731 pcl_conversions::toPCL(input_cloud->header, points_on_edges->header);
00732 for (size_t j = 0; j < perpendicular_triples.size(); j++) {
00733 pcl::PointCloud<PointT>::Ptr points_on_edge(new pcl::PointCloud<PointT>);
00734 Cube::Ptr cube = cubeFromIndicesAndCoefficients(
00735 cloud,
00736 perpendicular_triples[j],
00737 points_on_edge);
00738 *points_on_edges = *points_on_edges + *points_on_edge;
00739 cubes.push_back(cube);
00740 }
00741 pub_debug_filtered_cloud_.publish(points_on_edges);
00742 }
00743 }
00744
00745 if (cubes.size() > 0) {
00746 for (size_t i = 0; i < cubes.size(); i++) {
00747
00748 jsk_recognition_msgs::BoundingBox ros_box = cubes[i]->toROSMsg();
00749 ros_box.header = input_cloud->header;
00750 box_array.boxes.push_back(ros_box);
00751 pose_array.poses.push_back(ros_box.pose);
00752 }
00753 pub_.publish(box_array);
00754 pub_pose_array_.publish(pose_array);
00755 }
00756 }
00757
00758 bool EdgebasedCubeFinder::isPerpendicularVector(
00759 const Eigen::Vector3f& a,
00760 const Eigen::Vector3f& b)
00761 {
00762 double dot = a.normalized().dot(b.normalized());
00763 if (fabs(dot) >= 1.0) {
00764 return false;
00765 }
00766 else {
00767 double theta = fabs(acos(dot));
00768 JSK_NODELET_INFO("theta: %f", pcl::rad2deg(theta));
00769 if (fabs(theta - M_PI / 2.0) < pcl::deg2rad(20.0)) {
00770 return true;
00771 }
00772 else {
00773 return false;
00774 }
00775 }
00776 }
00777
00778 EdgebasedCubeFinder::EdgeRelation EdgebasedCubeFinder::perpendicularEdgeTriple(
00779 const Line& edge_a,
00780 const Line& edge_b,
00781 const Line& edge_c)
00782 {
00783 Eigen::Vector3f a_b_normal, a_c_normal;
00784 edge_a.parallelLineNormal(edge_b, a_b_normal);
00785 edge_a.parallelLineNormal(edge_c, a_c_normal);
00786 if (isPerpendicularVector(a_b_normal, a_c_normal)) {
00787 return A_PERPENDICULAR;
00788 }
00789 else {
00790 Eigen::Vector3f b_a_normal, b_c_normal;
00791 edge_b.parallelLineNormal(edge_a, b_a_normal);
00792 edge_b.parallelLineNormal(edge_c, b_c_normal);
00793 if (isPerpendicularVector(b_a_normal, b_c_normal)) {
00794 return B_PERPENDICULAR;
00795 }
00796 else {
00797 Eigen::Vector3f c_a_normal, c_b_normal;
00798 edge_c.parallelLineNormal(edge_a, c_a_normal);
00799 edge_c.parallelLineNormal(edge_b, c_b_normal);
00800 if (isPerpendicularVector(c_a_normal, c_b_normal)) {
00801 return C_PERPENDICULAR;
00802 }
00803 else {
00804 return NOT_PERPENDICULAR;
00805 }
00806 }
00807 }
00808 }
00809
00810 std::vector<IndicesCoefficientsTriple>
00811 EdgebasedCubeFinder::filterPerpendicularEdgeTriples(
00812 const std::vector<IndicesCoefficientsTriple>& triples)
00813 {
00814 std::vector<IndicesCoefficientsTriple> ret;
00815 for (size_t i = 0; i < triples.size(); i++) {
00816 pcl::ModelCoefficients::Ptr a_coefficients
00817 = triples[i].get<1>().get<0>();
00818 pcl::ModelCoefficients::Ptr b_coefficients
00819 = triples[i].get<1>().get<1>();
00820 pcl::ModelCoefficients::Ptr c_coefficients
00821 = triples[i].get<1>().get<2>();
00822 Line::Ptr edge_a
00823 = Line::fromCoefficients(a_coefficients->values);
00824 Line::Ptr edge_b
00825 = Line::fromCoefficients(b_coefficients->values);
00826 Line::Ptr edge_c
00827 = Line::fromCoefficients(c_coefficients->values);
00828
00829 EdgeRelation relation = perpendicularEdgeTriple(*edge_a,
00830 *edge_b,
00831 *edge_c);
00832 if (relation != NOT_PERPENDICULAR) {
00833
00834 if (relation == A_PERPENDICULAR) {
00835 ret.push_back(triples[i]);
00836 }
00837 else if (relation == B_PERPENDICULAR) {
00838 IndicesCoefficientsTriple new_triple
00839 = boost::make_tuple(
00840 boost::make_tuple(triples[i].get<0>().get<1>(),
00841 triples[i].get<0>().get<0>(),
00842 triples[i].get<0>().get<2>()),
00843 boost::make_tuple(
00844 triples[i].get<1>().get<1>(),
00845 triples[i].get<1>().get<0>(),
00846 triples[i].get<1>().get<2>()));
00847 ret.push_back(new_triple);
00848 }
00849 else if (relation == C_PERPENDICULAR) {
00850 IndicesCoefficientsTriple new_triple
00851 = boost::make_tuple(
00852 boost::make_tuple(triples[i].get<0>().get<2>(),
00853 triples[i].get<0>().get<0>(),
00854 triples[i].get<0>().get<1>()),
00855 boost::make_tuple(
00856 triples[i].get<1>().get<2>(),
00857 triples[i].get<1>().get<0>(),
00858 triples[i].get<1>().get<1>()));
00859 ret.push_back(new_triple);
00860 }
00861 }
00862 }
00863 return ret;
00864 }
00865
00866 void EdgebasedCubeFinder::estimate2(
00867 const sensor_msgs::PointCloud2::ConstPtr& input_cloud,
00868 const jsk_recognition_msgs::ParallelEdgeArray::ConstPtr& input_edges)
00869 {
00870 boost::mutex::scoped_lock lock(mutex_);
00871 pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
00872 pcl::PointCloud<PointT>::Ptr all_filtered_cloud (new pcl::PointCloud<PointT>);
00873 pcl::fromROSMsg(*input_cloud, *cloud);
00874 visualization_msgs::Marker marker;
00875 jsk_recognition_msgs::PolygonArray polygon_array;
00876 std::vector<pcl::PointIndices::Ptr> candidate_cluster_indices;
00877
00878 polygon_array.header = input_cloud->header;
00879 marker.pose.orientation.w = 1.0;
00880 marker.color.a = 1.0;
00881 marker.color.r = 1.0;
00882 marker.header = input_cloud->header;
00883 marker.scale.x = 0.01;
00884 marker.type = visualization_msgs::Marker::LINE_LIST;
00885 JSK_NODELET_INFO("%lu parallel edge groups", input_edges->edge_groups.size());
00886 geometry_msgs::PoseArray pose_array;
00887 pose_array.header = input_cloud->header;
00888 jsk_recognition_msgs::BoundingBoxArray ros_output;
00889 ros_output.header = input_cloud->header;
00890
00891 for (size_t i = 0; i < input_edges->edge_groups.size(); i++) {
00892 jsk_recognition_msgs::ParallelEdge parallel_edge = input_edges->edge_groups[i];
00893
00894 if (parallel_edge.cluster_indices.size() <= 1) {
00895 JSK_NODELET_ERROR("parallel edge group has only %lu edges",
00896 parallel_edge.cluster_indices.size());
00897 continue;
00898 }
00899 JSK_NODELET_INFO("%lu parallel edge groups has %lu edges",
00900 i, parallel_edge.cluster_indices.size());
00902
00904 std::vector<pcl::PointIndices::Ptr> edges
00905 = pcl_conversions::convertToPCLPointIndices(
00906 parallel_edge.cluster_indices);
00907 std::vector<pcl::ModelCoefficients::Ptr> coefficients
00908 = pcl_conversions::convertToPCLModelCoefficients(
00909 parallel_edge.coefficients);
00910
00911 std::vector<IndicesPair> pairs = combinateIndices(edges);
00912 std::vector<CoefficientsPair> coefficients_pair
00913 = combinateCoefficients(coefficients);
00914 std::vector<IndicesPair> filtered_indices_pairs;
00915 std::vector<CoefficientsPair> filtered_coefficients_pairs;
00916
00917 filtered_indices_pairs = pairs;
00918 filtered_coefficients_pairs = coefficients_pair;
00919
00920
00921
00922
00923
00924 std::vector<ConvexPolygon::Ptr> convexes;
00925 for (size_t j = 0; j < filtered_coefficients_pairs.size(); j++) {
00926 ConvexPolygon::Ptr convex
00927 = convexFromPairs(cloud, filtered_coefficients_pairs[j],
00928 pairs[j]);
00929 convexes.push_back(convex);
00930 }
00931 std::vector<int> filtered_indices;
00932 filterBasedOnConvex(cloud, convexes, filtered_indices);
00933
00934 pcl::PointIndices::Ptr filtered_cube_candidate_indices(new pcl::PointIndices);
00935 for (size_t j = 0; j < filtered_indices.size(); j++) {
00936 int index = filtered_indices[j];
00937 ConvexPolygon::Ptr target_convex = convexes[index];
00938 IndicesPair target_edge_indices_pair
00939 = filtered_indices_pairs[index];
00940 CoefficientsPair target_edge_coefficients_pair
00941 = filtered_coefficients_pairs[index];
00942
00943
00944 pcl::PointIndices::Ptr filtered_indices
00945 = preparePointCloudForRANSAC(
00946 target_convex, target_edge_coefficients_pair, cloud);
00947
00948
00949 filtered_cube_candidate_indices
00950 = addIndices(*filtered_cube_candidate_indices,
00951 *filtered_indices);
00952
00953
00954
00955
00956
00957
00958
00959
00960
00961
00962
00963
00964
00965
00966
00967
00968
00969
00970
00971
00972
00973
00974
00975
00976
00977
00978
00979
00980
00981
00982
00983
00984
00985
00986
00987
00988
00989
00990
00991
00992
00993
00994
00995
00996
00997
00998
00999
01000
01001
01002
01003
01004
01005
01006
01007
01008
01009
01010
01011
01012
01013
01014
01015
01016
01017
01018
01019
01020
01021
01022
01023
01024
01025
01026
01027
01028
01029
01030
01031
01032
01033
01034
01035
01036
01037
01038
01039
01040
01041
01042
01043
01044
01045
01046
01047
01048
01049 }
01050 candidate_cluster_indices.push_back(filtered_cube_candidate_indices);
01051
01052
01053 pcl::PointIndices::Ptr first_inliers(new pcl::PointIndices);
01054 pcl::ModelCoefficients::Ptr first_coefficients(new pcl::ModelCoefficients);
01055 ConvexPolygon::Ptr first_polygon
01056 = estimateConvexPolygon(
01057 cloud,
01058 filtered_cube_candidate_indices,
01059 first_coefficients,
01060 first_inliers);
01061 if (first_polygon) {
01062 geometry_msgs::PolygonStamped first_polygon_ros;
01063 first_polygon_ros.polygon = first_polygon->toROSMsg();
01064 first_polygon_ros.header = input_cloud->header;
01065 polygon_array.polygons.push_back(first_polygon_ros);
01066 }
01067
01068
01069
01070
01071
01072
01073
01074
01075
01076
01077
01078
01079
01080
01081
01082
01083
01084
01085
01086
01087
01088
01089
01090
01091
01092
01093
01094
01095
01096
01097
01098
01099
01100
01101
01102 }
01103 pub_.publish(ros_output);
01104 pub_pose_array_.publish(pose_array);
01105 pub_debug_marker_.publish(marker);
01106 sensor_msgs::PointCloud2 ros_cloud;
01107 pcl::toROSMsg(*all_filtered_cloud, ros_cloud);
01108 ros_cloud.header = input_cloud->header;
01109 pub_debug_filtered_cloud_.publish(ros_cloud);
01110 pub_debug_polygons_.publish(polygon_array);
01111
01112
01113 jsk_recognition_msgs::ClusterPointIndices ros_cluster_indices;
01114 ros_cluster_indices.header = input_cloud->header;
01115 for (size_t i = 0; i < candidate_cluster_indices.size(); i++) {
01116 PCLIndicesMsg indices_msg;
01117 indices_msg.header = input_cloud->header;
01118 indices_msg.indices = candidate_cluster_indices[i]->indices;
01119 ros_cluster_indices.cluster_indices.push_back(indices_msg);
01120 }
01121 pub_debug_clusers_.publish(ros_cluster_indices);
01122 }
01123
01124 std::vector<IndicesCoefficientsTriple>
01125 EdgebasedCubeFinder::tripleIndicesAndCoefficients(
01126 const std::vector<pcl::PointIndices::Ptr>& indices,
01127 const std::vector<pcl::ModelCoefficients::Ptr>& coefficients)
01128 {
01129 if (indices.size() != coefficients.size()) {
01130 JSK_NODELET_ERROR("size of indices and coefficients are not same");
01131 return std::vector<IndicesCoefficientsTriple>();
01132 }
01133
01134 if (indices.size() <= 2 && coefficients.size() <= 2) {
01135 JSK_NODELET_WARN("[EdgebasedCubeFinder::tripleIndicesAndCoefficients] no enough canddiates");
01136 return std::vector<IndicesCoefficientsTriple>();
01137 }
01138 std::vector<IndicesCoefficientsTriple> ret;
01139 for (size_t i = 0; i < indices.size() - 2; i++) {
01140 for (size_t j = i + 1; j < indices.size() - 1; j++) {
01141 for (size_t k = j + 1; k < indices.size(); k++) {
01142 IndicesTriple indices_triple
01143 = boost::make_tuple(indices[i],
01144 indices[j],
01145 indices[k]);
01146 CoefficientsTriple coefficients_triple
01147 = boost::make_tuple(coefficients[i],
01148 coefficients[j],
01149 coefficients[k]);
01150 IndicesCoefficientsTriple indices_coefficients_triple
01151 = boost::make_tuple(indices_triple,
01152 coefficients_triple);
01153 ret.push_back(indices_coefficients_triple);
01154 }
01155 }
01156 }
01157 return ret;
01158 }
01159
01160 ConvexPolygon::Ptr EdgebasedCubeFinder::estimateConvexPolygon(
01161 const pcl::PointCloud<PointT>::Ptr cloud,
01162 const pcl::PointIndices::Ptr indices,
01163 pcl::ModelCoefficients::Ptr coefficients,
01164 pcl::PointIndices::Ptr inliers)
01165 {
01167
01169 pcl::SACSegmentation<PointT> seg;
01170 seg.setOptimizeCoefficients (true);
01171 seg.setModelType (pcl::SACMODEL_PLANE);
01172 seg.setMethodType (pcl::SAC_RANSAC);
01173 seg.setInputCloud(cloud);
01174 seg.setIndices(indices);
01175 seg.setDistanceThreshold(0.003);
01176 seg.segment(*inliers, *coefficients);
01178
01180 if (inliers->indices.size() > 0) {
01181 return convexFromCoefficientsAndInliers<PointT>(
01182 cloud, inliers, coefficients);
01183 }
01184 else {
01185 return ConvexPolygon::Ptr();
01186 }
01187 }
01188
01189 std::vector<IndicesPair> EdgebasedCubeFinder::combinateIndices(
01190 const std::vector<pcl::PointIndices::Ptr>& indices)
01191 {
01192 std::vector<IndicesPair> ret;
01193 for(size_t i = 0; i < indices.size() - 1; i++) {
01194 for (size_t j = i + 1; j < indices.size(); j++) {
01195 IndicesPair pair = boost::make_tuple(indices[i],
01196 indices[j]);
01197 ret.push_back(pair);
01198 }
01199 }
01200 return ret;
01201 }
01202
01203 std::vector<CoefficientsPair> EdgebasedCubeFinder::combinateCoefficients(
01204 const std::vector<pcl::ModelCoefficients::Ptr>& coefficients)
01205 {
01206 std::vector<CoefficientsPair> ret;
01207 for(size_t i = 0; i < coefficients.size() - 1; i++) {
01208 for (size_t j = i + 1; j < coefficients.size(); j++) {
01209 CoefficientsPair pair = boost::make_tuple(coefficients[i],
01210 coefficients[j]);
01211 ret.push_back(pair);
01212 }
01213 }
01214 return ret;
01215 }
01216 }
01217
01218 #include <pluginlib/class_list_macros.h>
01219 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::EdgebasedCubeFinder, nodelet::Nodelet);
01220