38 #include <pcl/common/common.h>
39 #include <jsk_topic_tools/log_utils.h>
46 pcl::PointNormal min_pt, max_pt;
47 pcl::getMinMax3D(
cloud, min_pt, max_pt);
49 Eigen::Vector3f diff = max_pt.getVector3fMap() - min_pt.getVector3fMap();
51 size_t x_num = std::ceil(diff[0] /
grid_size_) + 1;
52 size_t y_num = std::ceil(diff[1] /
grid_size_) + 1;
54 mat_ = cv::Mat(y_num, x_num, CV_8U);
55 for (
size_t xi = 0; xi < x_num; xi++) {
57 for (
size_t yi = 0; yi < y_num; yi++) {
61 for (
size_t i = 0;
i <
cloud.points.size();
i++) {
62 pcl::PointNormal
p =
cloud.points[
i];
63 if (isnan(
p.x) || isnan(
p.y) || isnan(
p.z)) {
67 if (
index.x >= x_num) {
70 if (
index.y >= y_num) {
78 const Eigen::Vector3f& p0,
const Eigen::Vector3f& p1,
79 const Eigen::Vector3f& p2,
const Eigen::Vector3f& p3,
80 pcl::PointIndices& out_indices)
84 out_indices.indices.clear();
85 for (
size_t i = 0;
i < cell_indices.size();
i++) {
88 cell->fill(out_indices.indices);
98 indices.indices = std::vector<int>(cell_indices.begin(), cell_indices.end());
102 const Eigen::Vector3f& p0,
const Eigen::Vector3f& p1)
112 int dx = std::abs(x1 - x0);
113 int dy = std::abs(y1 - y0);
129 ret.push_back(cv::Point(x0, y0));
130 if (x0 == x1 && y0 == y1) {
146 const Eigen::Vector3f& p0,
const Eigen::Vector3f& p1,
147 const Eigen::Vector3f& p2,
const Eigen::Vector3f& p3)
158 return fill(box_indices);
164 mat_ = cv::Scalar::all(0);
165 std::vector<cv::Point> pts(filled.size());
166 for (
size_t i = 0;
i < filled.size();
i++) {
167 pts[
i] = cv::Point(filled[i].x, filled[i].y);
169 cv::fillConvexPoly(
mat_, pts, cv::Scalar(255));
171 cv::Rect bbox = cv::boundingRect(cv::Mat(pts));
173 ret.reserve(filled.size());
174 for (
size_t j = 0; j <= bbox.height; j++) {
176 for (
size_t i = 0;
i <= bbox.width;
i++) {
178 if (
mat_.at<
unsigned char>(y, x) == 255) {
179 ret.push_back(cv::Point(
x,
y));
187 const Eigen::Vector3f& p2,
const Eigen::Vector3f& p3)
194 for (
size_t i = 0;
i <
a.size();
i++) {
197 for (
size_t i = 0;
i <
b.size();
i++) {
198 ret[
i +
a.size()] =
b[
i];
200 for (
size_t i = 0;
i <
c.size();
i++) {
201 ret[
i +
a.size() +
b.size()] =
c[
i];
203 for (
size_t i = 0;
i <
d.size();
i++) {
204 ret[
i+
a.size() +
b.size() +
c.size()] =
d[
i];
218 for (
size_t i = 0;
i < pixels.size();
i++) {
220 mat.at<
char>(
id.y,
id.x) =
char(255);