void PC_to_Mat(PointCloudT::Ptr &cloud, cv::Mat &result){
if (cloud->isOrganized()) {
std::cout << "PointCloud is organized..." << std::endl;
result = cv::Mat(cloud->height, cloud->width, CV_8UC3);
if (!cloud->empty()) {
for (int h=0; h<result.rows; h++) {
for (int w=0; w<result.cols; w++) {
PointT point = cloud->at(w, h);
Eigen::Vector3i rgb = point.getRGBVector3i();
result.at<cv::Vec3b>(h,w)[0] = rgb[2];
result.at<cv::Vec3b>(h,w)[1] = rgb[1];
result.at<cv::Vec3b>(h,w)[2] = rgb[0];
}
}
}
}
}
void filter_PC_from_BB(PointCloudT::Ptr &cloud, cv::Mat &result, int x, int y, int width, int height){
const float bad_point = std::numeric_limits<float>::quiet_NaN();
if (cloud->isOrganized()) {
std::cout << "PointCloud is organized..." << std::endl;
result = cv::Mat(cloud->height, cloud->width, CV_8UC3);
if (!cloud->empty()) {
for (int h=0; h<result.rows; h++) {
for (int w=0; w<result.cols; w++) {
// Check if in bounding window
if ( (h>y && h<(y+height)) && ((w > x) && w < (x+width)) ){
// do nothing
} else {
// remove point
//PointT point = cloud->at(w, h);
//cloud->at(w, h);
cloud->at(w, h).x = bad_point;
cloud->at(w, h).y = bad_point;
cloud->at(w, h).z = bad_point;
cloud->at(w, h).r = bad_point;
cloud->at(w, h).g = bad_point;
cloud->at(w, h).b = bad_point;
}
}
}
}
}
}