ann_grid.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2015, 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_footstep_planner/ann_grid.h"
00037 
00038 #include <pcl/common/common.h>
00039 #include <jsk_topic_tools/log_utils.h>
00040 
00041 namespace jsk_footstep_planner
00042 {
00043   void ANNGrid::build(const pcl::PointCloud<pcl::PointNormal>& cloud)
00044   {
00045     cells_.clear();
00046     pcl::PointNormal min_pt, max_pt;
00047     pcl::getMinMax3D(cloud, min_pt, max_pt);
00048     min_point_ = min_pt.getVector3fMap();
00049     Eigen::Vector3f diff = max_pt.getVector3fMap() - min_pt.getVector3fMap();
00050 
00051     size_t x_num = std::ceil(diff[0] / grid_size_) + 1;
00052     size_t y_num = std::ceil(diff[1] / grid_size_) + 1;
00053     cells_.resize(x_num);
00054     mat_ = cv::Mat(y_num, x_num, CV_8U);
00055     for (size_t xi = 0; xi < x_num; xi++) {
00056       cells_[xi].resize(y_num);
00057       for (size_t yi = 0; yi < y_num; yi++) {
00058         cells_[xi][yi] = ANNGridCell::Ptr(new ANNGridCell);
00059       }
00060     }
00061     for (size_t i = 0; i < cloud.points.size(); i++) {
00062       pcl::PointNormal p = cloud.points[i];
00063       if (isnan(p.x) || isnan(p.y) || isnan(p.z)) {
00064         continue;
00065       }
00066       Index index = pointToIndex(p);
00067       if (index.x >= x_num) {
00068         ROS_FATAL("index.x exceeds x_num: %d > %lu", index.x, x_num);
00069       }
00070       if (index.y >= y_num) {
00071         ROS_FATAL("indey.y eyceeds y_num: %d > %lu", index.y, y_num);
00072       }
00073       cells_[index.x][index.y]->add(i);
00074     }
00075   }
00076 
00077   void ANNGrid::approximateSearchInBox(
00078     const Eigen::Vector3f& p0, const Eigen::Vector3f& p1,
00079     const Eigen::Vector3f& p2, const Eigen::Vector3f& p3,
00080     pcl::PointIndices& out_indices)
00081   {
00082     IndexArray cell_indices = fillByBox(p0, p1, p2, p3);
00083     //ANNGridCell::Indices point_indices;
00084     out_indices.indices.clear();
00085     for (size_t i = 0; i < cell_indices.size(); i++) {
00086       ANNGridCell::Ptr cell = getCell(cell_indices[i].x, cell_indices[i].y);
00087       if (cell) {
00088         cell->fill(out_indices.indices);
00089       }
00090     }
00091     //out_indices.indices = std::vector<int>(point_indices.begin(), point_indices.end());
00092   }
00093   
00094   void ANNGrid::approximateSearch(const Eigen::Vector3f& v, pcl::PointIndices& indices)
00095   {
00096     Index index = pointToIndex(v);
00097     ANNGridCell::Indices cell_indices = getCell(index.x, index.y)->get();
00098     indices.indices = std::vector<int>(cell_indices.begin(), cell_indices.end());
00099   }
00100 
00101   ANNGrid::IndexArray ANNGrid::bresenham(
00102     const Eigen::Vector3f& p0, const Eigen::Vector3f& p1)
00103   {
00104     IndexArray ret;
00105     Index i0 = pointToIndex(p0);
00106     Index i1 = pointToIndex(p1);
00107     
00108     int x0 = i0.x;
00109     int y0 = i0.y;
00110     int x1 = i1.x;
00111     int y1 = i1.y;
00112     int dx = std::abs(x1 - x0);
00113     int dy = std::abs(y1 - y0);
00114     int sx, sy;
00115     if (x0 < x1) {
00116       sx = 1;
00117     }
00118     else {
00119       sx = -1;
00120     }
00121     if (y0 < y1) {
00122       sy = 1;
00123     }
00124     else {
00125       sy = -1;
00126     }
00127     int err = dx - dy;
00128     while (1) {
00129       ret.push_back(cv::Point(x0, y0));
00130       if (x0 == x1 && y0 == y1) {
00131         return ret;
00132       }
00133       int e2 = 2 * err;
00134       if (e2 > -dy) {
00135         err = err - dy;
00136         x0 = x0 + sx;
00137       }
00138       if (e2 < dx) {
00139         err = err + dx;
00140         y0 = y0 + sy;
00141       }
00142     }
00143   }
00144 
00145   ANNGrid::IndexArray ANNGrid::fillByBox(
00146     const Eigen::Vector3f& p0, const Eigen::Vector3f& p1,
00147     const Eigen::Vector3f& p2, const Eigen::Vector3f& p3)
00148   {
00149     IndexArray box_indices(4);
00150     Index i0 = pointToIndex(p0);
00151     Index i1 = pointToIndex(p1);
00152     Index i2 = pointToIndex(p2);
00153     Index i3 = pointToIndex(p3);
00154     box_indices[0] = i0;
00155     box_indices[1] = i1;
00156     box_indices[2] = i2;
00157     box_indices[3] = i3;
00158     return fill(box_indices);
00159   }
00160   
00161   ANNGrid::IndexArray ANNGrid::fill(const IndexArray& filled)
00162   {
00163     // clear by 0
00164     mat_ = cv::Scalar::all(0);
00165     std::vector<cv::Point> pts(filled.size());
00166     for (size_t i = 0; i < filled.size(); i++) {
00167       pts[i] = cv::Point(filled[i].x, filled[i].y);
00168     }
00169     cv::fillConvexPoly(mat_, pts, cv::Scalar(255));
00170     // Compute bounding box
00171     cv::Rect bbox = cv::boundingRect(cv::Mat(pts));
00172     IndexArray ret;
00173     ret.reserve(filled.size());
00174     for (size_t j = 0; j <= bbox.height; j++) {
00175       int y = bbox.y + j;
00176       for (size_t i = 0; i <= bbox.width; i++) {
00177         int x = bbox.x + i;
00178         if (mat_.at<unsigned char>(y, x) == 255) {
00179           ret.push_back(cv::Point(x, y));
00180         }
00181       }
00182     }
00183     return ret;
00184   }
00185   
00186   ANNGrid::IndexArray ANNGrid::box(const Eigen::Vector3f& p0, const Eigen::Vector3f& p1,
00187                                    const Eigen::Vector3f& p2, const Eigen::Vector3f& p3)
00188   {
00189     IndexArray a = bresenham(p0, p1);
00190     IndexArray b = bresenham(p1, p2);
00191     IndexArray c = bresenham(p2, p3);
00192     IndexArray d = bresenham(p3, p0);
00193     IndexArray ret(a.size() + b.size() + c.size() + d.size());
00194     for (size_t i = 0; i < a.size(); i++) {
00195       ret[i] = a[i];
00196     }
00197     for (size_t i = 0; i < b.size(); i++) {
00198       ret[i + a.size()] = b[i];
00199     }
00200     for (size_t i = 0; i < c.size(); i++) {
00201       ret[i + a.size() + b.size()] = c[i];
00202     }
00203     for (size_t i = 0; i < d.size(); i++) {
00204       ret[i+ a.size() + b.size() + c.size()] = d[i];
00205     }
00206     return ret;
00207   }
00208   
00209   void ANNGrid::toImage(cv::Mat& mat)
00210   {
00211     mat = cv::Mat::zeros(cells_[0].size(), cells_.size(), CV_8U);
00212   }
00213   
00214   void ANNGrid::toImage(cv::Mat& mat, const IndexArray& pixels)
00215   {
00216     // Initialize by black
00217     toImage(mat);
00218     for (size_t i = 0; i < pixels.size(); i++) {
00219       Index id = pixels[i];
00220       mat.at<char>(id.y, id.x) = char(255);
00221     }
00222   }
00223 }


jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Fri Apr 19 2019 03:45:28