void removeLowConfidencePoints(cv::Mat& confidence_image, int threshold, PointCloudT::Ptr& cloud)
{
for (int i=0;i<cloud->height;i++)
{
for (int j=0;j<cloud->width;j++)
{
if (confidence_image.at<unsigned char>(i,j) < threshold)
{
cloud->at(j,i).x = std::numeric_limits<float>::quiet_NaN();
cloud->at(j,i).y = std::numeric_limits<float>::quiet_NaN();
cloud->at(j,i).z = std::numeric_limits<float>::quiet_NaN();
confidence_image.at<unsigned char>(i,j) = 0; // just for visualization
}
else
confidence_image.at<unsigned char>(i,j) = 255; // just for visualization
}
}
cloud->is_dense = false;
}
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;
}
}
}
}
}
}
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];
}
}
}
}
}