37 #ifndef JSK_FOOTSTEP_PLANNER_ANN_GRID_H_
38 #define JSK_FOOTSTEP_PLANNER_ANN_GRID_H_
40 #include <boost/tuple/tuple.hpp>
41 #include <boost/unordered/unordered_set.hpp>
42 #include <pcl/point_types.h>
43 #include <pcl/point_cloud.h>
44 #include <pcl/PointIndices.h>
46 #include <opencv2/opencv.hpp>
56 typedef boost::unordered_set<size_t>
Indices;
59 virtual void add(
size_t i)
74 virtual void fill(std::vector<int>& out)
97 typedef cv::Point
Index;
104 virtual void build(
const pcl::PointCloud<pcl::PointNormal>&
cloud);
108 Eigen::Vector3f
v =
p.getVector3fMap();
110 int i = (int)std::floor(translated_point[0] /
grid_size_);
111 int j = (int)std::floor(translated_point[1] /
grid_size_);
112 return cv::Point(i, j);
118 int i = (int)std::floor(translated_point[0] /
grid_size_);
120 return cv::Point(i, j);
139 virtual void approximateSearch(
const Eigen::Vector3f& v, pcl::PointIndices& indices);
144 const Eigen::Vector3f& p0,
const Eigen::Vector3f& p1,
145 const Eigen::Vector3f& p2,
const Eigen::Vector3f& p3);
147 const Eigen::Vector3f& p0,
const Eigen::Vector3f& p1,
148 const Eigen::Vector3f& p2,
const Eigen::Vector3f& p3,
149 pcl::PointIndices& indices);
151 virtual IndexArray box(
const Eigen::Vector3f& p0,
const Eigen::Vector3f& p1,
152 const Eigen::Vector3f& p2,
const Eigen::Vector3f& p3);
153 virtual void toImage(cv::Mat& mat);
158 std::vector<std::vector<ANNGridCell::Ptr> >
cells_;