| 228 | } |
| 229 | |
| 230 | void Segmentation::maskCloud(const Cloud::Ptr cloud, |
| 231 | cv::Mat mask, |
| 232 | Cloud::Ptr& outCloud, |
| 233 | unsigned char val, |
| 234 | bool dense) { |
| 235 | |
| 236 | |
| 237 | Cloud::Ptr tempCloud(new Cloud); |
| 238 | size_t numPoints = mask.rows * mask.cols; |
| 239 | assert((mask.rows*mask.cols) == (cloud->width*cloud->height)); |
| 240 | |
| 241 | for (int i = 0; i < numPoints; i++) { |
| 242 | size_t proj_idx = proj_ys[i] * _img_w + proj_xs[i]; |
| 243 | unsigned char m = mask.data[proj_idx * sizeof(unsigned char)]; |
| 244 | |
| 245 | if(m == val){ |
| 246 | tempCloud->points.push_back(cloud->points[i]); |
| 247 | } else if(dense){ |
| 248 | Point p; |
| 249 | p.x = p.y = p.z = std::numeric_limits<float>::quiet_NaN(); |
| 250 | tempCloud->points.push_back(p); |
| 251 | } |
| 252 | } |
| 253 | |
| 254 | pcl::copyPointCloud(*tempCloud, *outCloud); |
| 255 | if(dense){ |
| 256 | // destagger. TODO: Do this with mask |
| 257 | // Adapted from Chao's driver |
| 258 | if (_do_destagger){ |
| 259 | _destaggerCloud(tempCloud, outCloud); |
| 260 | } else { |
| 261 | outCloud = tempCloud; |
| 262 | } |
| 263 | outCloud->width = _img_w; |
| 264 | outCloud->height = _img_h; |
| 265 | outCloud->is_dense = true; |
| 266 | } else { |
| 267 | outCloud->width = outCloud->points.size(); |
| 268 | outCloud->height = 1; |
| 269 | outCloud->is_dense = false; |
| 270 | } |
| 271 | |
| 272 | // outCloud->header = cloud->header; |
| 273 | } |
| 274 | |
| 275 | void Segmentation::_mask(const float* output, const std::vector<size_t>& invalid_idxs, cv::Mat& maskImg){ |
| 276 | size_t channel_offset = _img_w * _img_h; |