| 72 | //////////////////////////////////////////////////////////////////////////////// |
| 73 | |
| 74 | bool |
| 75 | pcl::ihs::InputDataProcessing::segment(const CloudXYZRGBAConstPtr& cloud_in, |
| 76 | CloudXYZRGBNormalPtr& cloud_out, |
| 77 | CloudXYZRGBNormalPtr& cloud_discarded) const |
| 78 | { |
| 79 | if (!cloud_in) { |
| 80 | std::cerr << "ERROR in input_data_processing.cpp: Input cloud is invalid.\n"; |
| 81 | return (false); |
| 82 | } |
| 83 | if (!cloud_in->isOrganized()) { |
| 84 | std::cerr << "ERROR in input_data_processing.cpp: Input cloud must be organized.\n"; |
| 85 | return (false); |
| 86 | } |
| 87 | |
| 88 | if (!cloud_out) |
| 89 | cloud_out = CloudXYZRGBNormalPtr(new CloudXYZRGBNormal()); |
| 90 | if (!cloud_discarded) |
| 91 | cloud_discarded = CloudXYZRGBNormalPtr(new CloudXYZRGBNormal()); |
| 92 | |
| 93 | const unsigned int width = cloud_in->width; |
| 94 | const unsigned int height = cloud_in->height; |
| 95 | |
| 96 | // Calculate the normals |
| 97 | CloudNormalsPtr cloud_normals(new CloudNormals()); |
| 98 | normal_estimation_->setInputCloud(cloud_in); |
| 99 | normal_estimation_->compute(*cloud_normals); |
| 100 | |
| 101 | // Get the XYZ and HSV masks. |
| 102 | MatrixXb xyz_mask(height, width); |
| 103 | MatrixXb hsv_mask(height, width); |
| 104 | |
| 105 | // cm -> m for the comparison |
| 106 | const float x_min = .01f * x_min_; |
| 107 | const float x_max = .01f * x_max_; |
| 108 | const float y_min = .01f * y_min_; |
| 109 | const float y_max = .01f * y_max_; |
| 110 | const float z_min = .01f * z_min_; |
| 111 | const float z_max = .01f * z_max_; |
| 112 | |
| 113 | float h, s, v; |
| 114 | for (MatrixXb::Index r = 0; r < xyz_mask.rows(); ++r) { |
| 115 | for (MatrixXb::Index c = 0; c < xyz_mask.cols(); ++c) { |
| 116 | const PointXYZRGBA& xyzrgb = (*cloud_in)[r * width + c]; |
| 117 | const Normal& normal = (*cloud_normals)[r * width + c]; |
| 118 | |
| 119 | xyz_mask(r, c) = hsv_mask(r, c) = false; |
| 120 | |
| 121 | if (!std::isnan(xyzrgb.x) && !std::isnan(normal.normal_x) && xyzrgb.x >= x_min && |
| 122 | xyzrgb.x <= x_max && xyzrgb.y >= y_min && xyzrgb.y <= y_max && |
| 123 | xyzrgb.z >= z_min && xyzrgb.z <= z_max) { |
| 124 | xyz_mask(r, c) = true; |
| 125 | |
| 126 | this->RGBToHSV(xyzrgb.r, xyzrgb.g, xyzrgb.b, h, s, v); |
| 127 | if (h >= h_min_ && h <= h_max_ && s >= s_min_ && s <= s_max_ && v >= v_min_ && |
| 128 | v <= v_max_) { |
| 129 | if (!hsv_inverted_) |
| 130 | hsv_mask(r, c) = true; |
| 131 | } |
no test coverage detected