38 #include <pcl/common/common.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) {
68 ROS_FATAL(
"index.x exceeds x_num: %d > %lu", index.x, x_num);
70 if (index.y >= y_num) {
71 ROS_FATAL(
"indey.y eyceeds y_num: %d > %lu", index.y, y_num);
73 cells_[index.x][index.y]->add(i);
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)
193 IndexArray ret(a.size() + b.size() + c.size() + d.size());
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++) {
219 Index id = pixels[i];
220 mat.at<
char>(
id.y,
id.x) =
char(255);
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)