grid_map.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 #define BOOST_PARAMETER_MAX_ARITY 7
00036 #include "jsk_recognition_utils/grid_map.h"
00037 #include <boost/make_shared.hpp>
00038 #include <Eigen/Core>
00039 #include "jsk_recognition_utils/geo_util.h"
00040 #include <eigen_conversions/eigen_msg.h>
00041 #include <nodelet/nodelet.h>
00042 #include "jsk_recognition_utils/pcl_conversion_util.h"
00043 #include <pcl/surface/convex_hull.h>
00044 #include <jsk_topic_tools/log_utils.h>
00045 //#define DEBUG_GRID_MAP
00046 
00047 namespace jsk_recognition_utils
00048 {
00049   GridMap::GridMap(double resolution, const std::vector<float>& coefficients):
00050     resolution_(resolution), vote_(0)
00051   {
00052     normal_[0] = -coefficients[0];
00053     normal_[1] = -coefficients[1];
00054     normal_[2] = -coefficients[2];
00055     d_ = -coefficients[3];
00056     if (normal_.norm() != 1.0) {
00057       d_ = d_ / normal_.norm();
00058       normal_.normalize();
00059     }
00060     O_ = - d_ * normal_;
00061     // decide ex_ and ey_
00062     Eigen::Vector3f u(1, 0, 0);
00063     if (normal_ == u) {
00064       u[0] = 0; u[1] = 1; u[2] = 0;
00065     }
00066     ey_ = normal_.cross(u).normalized();
00067     ex_ = ey_.cross(normal_).normalized();
00068   }
00069   
00070   GridMap::~GridMap()
00071   {
00072     
00073   }
00074 
00075   GridIndex::Ptr GridMap::registerIndex(const int x, const int y)
00076   {
00077     ColumnIterator it = data_.find(x);
00078     if (it != data_.end()) {
00079       (it->second).insert(y);
00080     }
00081     else {
00082       RowIndices new_row;
00083       new_row.insert(y);
00084       data_[x] = new_row;
00085     }
00086     GridIndex::Ptr ret(new GridIndex(x, y));
00087     return ret;
00088   }
00089   
00090   GridIndex::Ptr GridMap::registerIndex(const GridIndex::Ptr& index)
00091   {
00092     return registerIndex(index->x, index->y);
00093   }
00094   
00095   void GridMap::registerPoint(const pcl::PointXYZRGB& point)
00096   {
00097     GridIndex::Ptr index (new GridIndex());
00098     pointToIndex(point, index);
00099     // check duplication
00100     registerIndex(index);
00101   }
00102   
00103   std::vector<GridIndex::Ptr> GridMap::registerLine(const pcl::PointXYZRGB& from,
00104                                                  const pcl::PointXYZRGB& to)
00105   {
00106 #ifdef DEBUG_GRID_MAP
00107     std::cout << "newline" << std::endl;
00108 #endif
00109     std::vector<GridIndex::Ptr> added_indices;
00110     //GridLine::Ptr new_line (new GridLine(from, to));
00111     //lines_.push_back(new_line);
00112     // count up all the grids which the line penetrates
00113     
00114     // 1. convert to y = ax + b style equation.
00115     // 2. move x from the start index to the end index and count up the y range
00116     // if it cannot be convert to y = ax + b style, it means the equation
00117     // is represented as x = c style.
00118     double from_x = from.getVector3fMap().dot(ex_) / resolution_;
00119     double from_y = from.getVector3fMap().dot(ey_) / resolution_;
00120     double to_x = to.getVector3fMap().dot(ex_) / resolution_;
00121     double to_y = to.getVector3fMap().dot(ey_) / resolution_;
00122 #ifdef DEBUG_GRID_MAP
00123     std::cout << "registering (" << (int)from_x << ", " << (int)from_y << ")" << std::endl;
00124     std::cout << "registering (" << (int)to_x << ", " << (int)to_y << ")" << std::endl;
00125 #endif
00126     added_indices.push_back(registerIndex(from_x, from_y));
00127     added_indices.push_back(registerIndex(to_x, to_y));
00128     if (from_x != to_x) {
00129       double a = (to_y - from_y) / (to_x - from_x);
00130       double b = - a * from_x + from_y;
00131 #ifdef DEBUG_GRID_MAP
00132       std::cout << "a: " << a << std::endl;
00133 #endif
00134       if (a == 0.0) {
00135 #ifdef DEBUG_GRID_MAP
00136         std::cout << "parallel to x" << std::endl;
00137 #endif
00138         int from_int_x = (int)from_x;
00139         int to_int_x = (int)to_x;
00140         int int_y = (int)from_y;
00141         if (from_int_x > to_int_x) {
00142           std::swap(from_int_x, to_int_x);
00143         }
00144         for (int ix = from_int_x; ix < to_int_x; ++ix) {
00145           added_indices.push_back(registerIndex(ix, int_y));
00146 #ifdef DEBUG_GRID_MAP
00147           std::cout << "registering (" << ix << ", " << int_y << ")" << std::endl;
00148 #endif
00149         }
00150       }
00151       else if (fabs(a) < 1.0) {
00152 #ifdef DEBUG_GRID_MAP
00153         std::cout << "based on x" << std::endl;
00154 #endif
00155         int from_int_x = (int)from_x;
00156         int to_int_x = (int)to_x;
00157         if (from_int_x > to_int_x) {
00158           std::swap(from_int_x, to_int_x);
00159         }
00160         
00161         for (int ix = from_int_x; ix < to_int_x; ++ix) {
00162           double y = a * ix + b;
00163           added_indices.push_back(registerIndex(ix, (int)y));
00164 #ifdef DEBUG_GRID_MAP
00165           std::cout << "registering (" << ix << ", " << (int)y << ")" << std::endl;
00166 #endif
00167         }
00168       }
00169       else {
00170 #ifdef DEBUG_GRID_MAP
00171         std::cout << "based on y" << std::endl;
00172 #endif
00173         int from_int_y = (int)from_y;
00174         int to_int_y = (int)to_y;
00175         if (from_int_y > to_int_y) {
00176           std::swap(from_int_y, to_int_y);
00177         }
00178         
00179         for (int iy = from_int_y; iy < to_int_y; ++iy) {
00180           double x = iy / a - b / a;
00181           added_indices.push_back(registerIndex((int)x, iy));
00182 #ifdef DEBUG_GRID_MAP
00183           std::cout << "registering (" << (int)x << ", " << iy << ")" << std::endl;
00184 #endif
00185         }
00186       }
00187       
00188     }
00189     else {
00190 #ifdef DEBUG_GRID_MAP
00191       std::cout << "parallel to y" << std::endl;
00192 #endif
00193       // the line is parallel to y
00194       int from_int_y = (int)from_y;
00195       int to_int_y = (int)to_y;
00196       int int_x = (int)from_x;
00197       if (from_int_y > to_int_y) {
00198         std::swap(from_int_y, to_int_y);
00199       }
00200       for (int iy = from_int_y; iy < to_int_y; ++iy) {
00201         added_indices.push_back(registerIndex(int_x, iy));
00202 #ifdef DEBUG_GRID_MAP
00203         std::cout << "registering (" << int_x << ", " << (int)iy << ")" << std::endl;
00204 #endif
00205       }
00206     }
00207     return added_indices;
00208   }
00209 
00210   void GridMap::registerPointCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud)
00211   {
00212     for (size_t i = 0; i < cloud->points.size(); i++) {
00213       registerPoint(cloud->points[i]);
00214       //ROS_INFO("registered point: [%f, %f, %f]", cloud->points[i].x, cloud->points[i].y, cloud->points[i].z);
00215     }
00216   }
00217   
00218   void GridMap::pointToIndex(const pcl::PointXYZRGB& point, GridIndex::Ptr index)
00219   {
00220     pointToIndex(point.getVector3fMap(), index);
00221   }
00222 
00223   void GridMap::pointToIndex(const Eigen::Vector3f& p, GridIndex::Ptr index)
00224   {
00225     index->x = (p - O_).dot(ex_) / resolution_;
00226     index->y = (p - O_).dot(ey_) / resolution_;
00227   }
00228 
00229   void GridMap::gridToPoint(GridIndex::Ptr index, Eigen::Vector3f& pos)
00230   {
00231     gridToPoint(*index, pos);
00232   }
00233 
00234   void GridMap::gridToPoint(const GridIndex& index, Eigen::Vector3f& pos)
00235   {
00236     //pos = resolution_ * (index.x * ex_ + index.y * ey_) + O_;
00237     pos = resolution_ * ((index.x + 0.5) * ex_ + (index.y + 0.5) * ey_) + O_;
00238   }
00239 
00240   void GridMap::gridToPoint2(const GridIndex& index, Eigen::Vector3f& pos)
00241   {
00242     //pos = resolution_ * ((index.x - 0.5) * ex_ + (index.y - 0.5) * ey_) + O_;
00243     pos = resolution_ * ((index.x - 0.0) * ex_ + (index.y - 0.0) * ey_) + O_;
00244   }
00245 
00246   
00247   bool GridMap::getValue(const int x, const int y)
00248   {
00249     // check line
00250     // for (size_t i = 0; i < lines_.size(); i++) {
00251     //   GridLine::Ptr line = lines_[i];
00252     //   Eigen::Vector3f A, B, C, D;
00253     //   gridToPoint2(GridIndex(x, y), A);
00254     //   gridToPoint2(GridIndex(x + 1, y), B);
00255     //   gridToPoint2(GridIndex(x + 1, y + 1), C);
00256     //   gridToPoint2(GridIndex(x, y + 1), D);
00257     //   bool penetrate = line->penetrateGrid(A, B, C, D);
00258     //   if (penetrate) {
00259     //   //   // printf("(%lf, %lf, %lf) - (%lf, %lf, %lf) penetrate (%d, %d)\n",
00260     //   //   //        line->from[0],line->from[1],line->from[2],
00261     //   //   //        line->to[0],line->to[1],line->to[2],
00262     //   //   //        x, y);
00263     //   //   //std::cout << "penetrate"
00264     //     return true;
00265     //   }
00266     // }
00267 
00268     ColumnIterator it = data_.find(x);
00269     if (it == data_.end()) {
00270       return false;
00271     }
00272     else {
00273       RowIndices c = it->second;
00274       if (c.find(y) == c.end()) {
00275         return false;
00276       }
00277       else {
00278         return true;
00279       }
00280     }
00281   }
00282   
00283   bool GridMap::getValue(const GridIndex& index)
00284   {
00285     return getValue(index.x, index.y);
00286   }
00287   
00288   bool GridMap::getValue(const GridIndex::Ptr& index)
00289   {
00290     return getValue(*index);
00291   }
00292 
00293   void GridMap::fillRegion(const GridIndex::Ptr start, std::vector<GridIndex::Ptr>& output)
00294   {
00295 #ifdef DEBUG_GRID_MAP
00296     std::cout << "filling " << start->x << ", " << start->y << std::endl;
00297 #endif
00298     output.push_back(start);
00299     registerIndex(start);
00300 #ifdef DEBUG_GRID_MAP
00301     if (abs(start->x) > 100 || abs(start->y) > 100) {
00302       //exit(1);
00303       std::cout << "force to quit" << std::endl;
00304       for (size_t i = 0; i < lines_.size(); i++) {
00305         GridLine::Ptr line = lines_[i];
00306         Eigen::Vector3f from = line->from;
00307         Eigen::Vector3f to = line->to;
00308 #ifdef DEBUG_GRID_MAP
00309         std::cout << "line[" << i << "]: "
00310                   << "[" << from[0] << ", " << from[1] << ", " << from[2] << "] -- "
00311                   << "[" << to[0] << ", " << to[1] << ", " << to[2] << std::endl;
00312 #endif
00313       }
00314       return;                   // force to quit
00315     }
00316 #endif
00317     GridIndex U(start->x, start->y + 1),
00318               D(start->x, start->y - 1),
00319               R(start->x + 1, start->y),
00320               L(start->x - 1, start->y);
00321     
00322     if (!getValue(U)) {
00323       fillRegion(boost::make_shared<GridIndex>(U), output);
00324     }
00325     if (!getValue(L)) {
00326       fillRegion(boost::make_shared<GridIndex>(L), output);
00327     }
00328     if (!getValue(R)) {
00329       fillRegion(boost::make_shared<GridIndex>(R), output);
00330     }
00331     if (!getValue(D)) {
00332       fillRegion(boost::make_shared<GridIndex>(D), output);
00333     }
00334     
00335   }
00336   
00337   void GridMap::fillRegion(const Eigen::Vector3f& start, std::vector<GridIndex::Ptr>& output)
00338   {
00339     GridIndex::Ptr start_index (new GridIndex);
00340     pointToIndex(start, start_index);
00341     fillRegion(start_index, output);
00342   }
00343   
00344   void GridMap::indicesToPointCloud(const std::vector<GridIndex::Ptr>& indices,
00345                                     pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud)
00346   {
00347     for (size_t i = 0; i < indices.size(); i++) {
00348       GridIndex::Ptr index = indices[i];
00349       Eigen::Vector3f point;
00350       pcl::PointXYZRGB new_point;
00351       gridToPoint(index, point);
00352       new_point.x = point[0];
00353       new_point.y = point[1];
00354       new_point.z = point[2];
00355       cloud->points.push_back(new_point);
00356     }
00357   }
00358 
00359   void GridMap::originPose(Eigen::Affine3f& output)
00360   {
00361     Eigen::Matrix3f rot_mat;
00362     rot_mat.col(0) = Eigen::Vector3f(ex_[0], ex_[1], ex_[2]);
00363     rot_mat.col(1) = Eigen::Vector3f(ey_[0], ey_[1], ey_[2]);
00364     rot_mat.col(2) = Eigen::Vector3f(normal_[0], normal_[1], normal_[2]);
00365     ROS_DEBUG("O: [%f, %f, %f]", O_[0], O_[1], O_[2]);
00366     ROS_DEBUG("ex: [%f, %f, %f]", ex_[0], ex_[1], ex_[2]);
00367     ROS_DEBUG("ey: [%f, %f, %f]", ey_[0], ey_[1], ey_[2]);
00368     ROS_DEBUG("normal: [%f, %f, %f]", normal_[0], normal_[1], normal_[2]);
00369     output = Eigen::Translation3f(O_) * Eigen::Quaternionf(rot_mat);
00370   }
00371   
00372   void GridMap::originPose(Eigen::Affine3d& output)
00373   {
00374     Eigen::Affine3f float_affine;
00375     originPose(float_affine);
00376     convertEigenAffine3(float_affine, output);
00377   }
00378   
00379   void GridMap::toMsg(jsk_recognition_msgs::SparseOccupancyGrid& grid)
00380   {
00381     grid.resolution = resolution_;
00382     // compute origin POSE from O and normal_, d_
00383     Eigen::Affine3d plane_pose;
00384     originPose(plane_pose);
00385     tf::poseEigenToMsg(plane_pose, grid.origin_pose);
00386     for (ColumnIterator it = data_.begin();
00387          it != data_.end();
00388          it++) {
00389       int column_index = it->first;
00390       RowIndices row_indices = it->second;
00391       jsk_recognition_msgs::SparseOccupancyGridColumn ros_column;
00392       ros_column.column_index = column_index;
00393       for (RowIterator rit = row_indices.begin();
00394            rit != row_indices.end();
00395            rit++) {
00396         jsk_recognition_msgs::SparseOccupancyGridCell cell;
00397         cell.row_index = *rit;
00398         cell.value = 1.0;
00399         ros_column.cells.push_back(cell);
00400       }
00401       grid.columns.push_back(ros_column);
00402     }
00403   }
00404 
00405   Plane GridMap::toPlane()
00406   {
00407     return Plane(normal_, d_);
00408   }
00409 
00410   Plane::Ptr GridMap::toPlanePtr()
00411   {
00412     Plane::Ptr ret (new Plane(normal_, d_));
00413     return ret;
00414   }
00415 
00416 
00417   std::vector<float> GridMap::getCoefficients()
00418   {
00419     std::vector<float> output;
00420     output.push_back(normal_[0]);
00421     output.push_back(normal_[1]);
00422     output.push_back(normal_[2]);
00423     output.push_back(d_);
00424     return output;
00425   }
00426 
00427   void GridMap::vote()
00428   {
00429     ++vote_;
00430   }
00431 
00432   unsigned int GridMap::getVoteNum()
00433   {
00434     return vote_;
00435   }
00436 
00437   void GridMap::setGeneration(unsigned int generation) {
00438     generation_ = generation;
00439   }
00440 
00441   unsigned int GridMap::getGeneration()
00442   {
00443     return generation_;
00444   }
00445   
00446   void GridMap::removeIndex(const GridIndex::Ptr& index)
00447   {
00448     int x = index->x;
00449     int y = index->y;
00450     ColumnIterator it = data_.find(x);
00451     if (it != data_.end()) {
00452       RowIterator rit = (it->second).find(y);
00453       if (rit != it->second.end()) {
00454         it->second.erase(rit);
00455       }
00456     }
00457   }
00458   bool GridMap::isBinsOccupied(const Eigen::Vector3f& p)
00459   {
00460     GridIndex::Ptr ret (new GridIndex());
00461     pointToIndex(p, ret);
00462     //ROS_INFO("checking (%d, %d)", ret->x, ret->y);
00463     return getValue(ret);
00464   }
00465 
00466   boost::tuple<int, int> GridMap::minMaxX()
00467   {
00468     int min_x = INT_MAX;
00469     int max_x = - INT_MAX;
00470     for (ColumnIterator it = data_.begin();
00471          it != data_.end();
00472          ++it) {
00473       int x = it->first;
00474       if (min_x > x) {
00475         min_x = x;
00476       }
00477       if (max_x < x) {
00478         max_x = x;
00479       }
00480     }
00481     return boost::make_tuple<int, int>(min_x, max_x);
00482   }
00483 
00484   boost::tuple<int, int> GridMap::minMaxY()
00485   {
00486     int min_y = INT_MAX;
00487     int max_y = - INT_MAX;
00488     for (ColumnIterator it = data_.begin();
00489          it != data_.end();
00490          ++it) {
00491       RowIndices row_indices = it->second;
00492       for (RowIterator rit = row_indices.begin();
00493            rit != row_indices.end();
00494            rit++) {
00495         int y = *rit;
00496         if (min_y > y) {
00497           min_y = y;
00498         }
00499         if (max_y < y) {
00500           max_y = y;
00501         }
00502       }
00503     }
00504     return boost::make_tuple<int, int>(min_y, max_y);
00505   }
00506   
00507   int GridMap::normalizedWidth()
00508   {
00509     boost::tuple<int, int> min_max_x = minMaxX();
00510     return min_max_x.get<1>() - min_max_x.get<0>();
00511   }
00512 
00513   int GridMap::normalizedHeight()
00514   {
00515     boost::tuple<int, int> min_max_y = minMaxY();
00516     return min_max_y.get<1>() - min_max_y.get<0>();
00517   }
00518 
00519   int GridMap::widthOffset()
00520   {
00521     boost::tuple<int, int> min_max_x = minMaxX();
00522     int min_x = min_max_x.get<0>();
00523     return - min_x;
00524   }
00525 
00526   int GridMap::heightOffset()
00527   {
00528     boost::tuple<int, int> min_max_y = minMaxY();
00529     int min_y = min_max_y.get<0>();
00530     return - min_y;
00531   }
00532 
00533   int GridMap::normalizedIndex(int width_offset, int height_offset,
00534                                int step,
00535                                int elem_size,
00536                                int original_x, int original_y)
00537   {
00538     int x = original_x + width_offset;
00539     int y = original_y + height_offset;
00540     return y * step + x * elem_size;
00541   }
00542   
00543   
00544   cv::Mat GridMap::toImage()
00545   {
00546     // initialize with black
00547     int width = normalizedWidth();
00548     int height = normalizedHeight();
00549     int width_offset = widthOffset();
00550     int height_offset = heightOffset();
00551     cv::Mat m = cv::Mat(width, height, CV_8UC1) * 0;
00552     // for all index
00553     for (ColumnIterator it = data_.begin();
00554          it != data_.end();
00555          ++it) {
00556       for (RowIterator rit = it->second.begin();
00557            rit != it->second.end();
00558            ++rit) {
00559         m.data[normalizedIndex(width_offset, height_offset,
00560                                m.step, m.elemSize(),
00561                                it->first, *rit)] = 255;
00562       }
00563     }
00564     
00565     return m;
00566   }
00567 
00568   bool GridMap::check4Neighbor(int x, int y) {
00569     if (getValue(x + 1, y) &&
00570         getValue(x + 1, y + 1) &&
00571         getValue(x - 1, y) &&
00572         getValue(x - 1, y - 1)) {
00573       return true;
00574     }
00575     else {
00576       return false;
00577     }
00578   }
00579   
00580   void GridMap::decreaseOne()
00581   {
00582     //Columns new_data;
00583     GridMap::Ptr new_map (new GridMap(resolution_, getCoefficients()));
00584     for (ColumnIterator it = data_.begin();
00585          it != data_.end();
00586          it++) {
00587       RowIndices row_indices = it->second;
00588       int x = it->first;
00589       for (RowIterator rit = row_indices.begin();
00590            rit != row_indices.end();
00591            rit++) {
00592         int y = *rit;
00593         if (check4Neighbor(x, y)) {
00594           new_map->registerIndex(x, y);
00595         }
00596       }
00597     }
00598     data_ = new_map->data_;
00599   }
00600   
00601   void GridMap::decrease(int i)
00602   {
00603     for (int ii = 0; ii < i; ii++) {
00604       decreaseOne();
00605     }
00606   }
00607 
00608   void GridMap::add(GridMap& other)
00609   {
00610     for (ColumnIterator it = other.data_.begin();
00611          it != other.data_.end();
00612          it++) {
00613       RowIndices row_indices = it->second;
00614       int x = it->first;
00615       for (RowIterator rit = row_indices.begin();
00616            rit != row_indices.end();
00617            rit++) {
00618         int y = *rit;
00619         Eigen::Vector3f pos;
00620         GridIndex index(x, y);
00621         other.gridToPoint(index, pos);
00622         pcl::PointXYZRGB p;
00623         pointFromVectorToXYZ<Eigen::Vector3f, pcl::PointXYZRGB>(pos, p);
00624         registerPoint(p);
00625       }
00626     }
00627   }
00628   
00629   pcl::PointCloud<pcl::PointXYZ>::Ptr GridMap::toPointCloud()
00630   {
00631     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
00632     for (ColumnIterator it = data_.begin();
00633          it != data_.end();
00634          it++) {
00635       RowIndices row_indices = it->second;
00636       int x = it->first;
00637       for (RowIterator rit = row_indices.begin();
00638            rit != row_indices.end();
00639            rit++) {
00640         int y = *rit;
00641         Eigen::Vector3f pos;
00642         GridIndex index(x, y);
00643         gridToPoint(index, pos);
00644         pcl::PointXYZ p;
00645         pointFromVectorToXYZ<Eigen::Vector3f, pcl::PointXYZ>(pos, p);
00646         cloud->points.push_back(p);
00647       }
00648     }
00649     return cloud;
00650   }
00651 
00652   ConvexPolygon::Ptr GridMap::toConvexPolygon()
00653   {
00654     // 1. build pointcloud
00655     // 2. compute convex hull
00656     // 3. return it as ConvexPolygon
00657     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = toPointCloud();
00658     pcl::ConvexHull<pcl::PointXYZ> chull;
00659     chull.setInputCloud(cloud);
00660     chull.setDimension(2);
00661     pcl::PointCloud<pcl::PointXYZ> chull_output;
00662     chull.reconstruct(chull_output);
00663     // convex chull_output to Vertices
00664     Vertices vs;
00665     for (size_t i = 0; i < chull_output.points.size(); i++) {
00666       Eigen::Vector3f v = chull_output.points[i].getVector3fMap();
00667       vs.push_back(v);
00668     }
00669     return ConvexPolygon::Ptr(new ConvexPolygon(vs));
00670   }
00671 
00672 }


jsk_recognition_utils
Author(s):
autogenerated on Tue Jul 2 2019 19:40:37