#pragma once
#include <iostream>
#include <iomanip>
#include <string>
#include <functional>
#include <algorithm>
#include <pcl/io/openni2_grabber.h>
#include <pcl/console/parse.h>
#include <boost/thread/thread.hpp>.
#include <pcl/common/common_headers.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/boundary.h>
#include <pcl/keypoints/harris_3d.h>
#include <pcl/surface/mls.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/surface/concave_hull.h>
#include <opencv2/opencv.hpp>
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
using namespace std;
using namespace cv;
class VolumDetectViewer
{
typedef pcl::visualization::PCLVisualizer PCLVisualizer;
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
typedef const pcl::PointCloud<pcl::PointXYZ>::ConstPtr ConstPointCloudConstPtr;
typedef pcl::PointCloud<pcl::PointXYZ>::Ptr PointCloudPtr;
typedef const pcl::PointCloud<pcl::PointXYZ>::Ptr ConstPointCloudPtr;
typedef pcl::search::KdTree<pcl::PointXYZ> KdTree;
typedef pcl::search::KdTree<pcl::PointXYZ>::Ptr KdTreePtr;
typedef pcl::PointCloud<pcl::Normal> Normal;
typedef pcl::PointCloud<pcl::Normal>::Ptr NormalPtr;
typedef pcl::PointCloud<pcl::Boundary> Boundary;
typedef pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointXYZ> MovingLeastSquares;
typedef pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> NormalEstimation;
typedef pcl::BoundaryEstimation<pcl::PointXYZ, pcl::Normal, pcl::Boundary> BoundaryEstimation;
typedef pcl::PassThrough<pcl::PointXYZ> PassThrough;
typedef pcl::VoxelGrid<pcl::PointXYZ> VoxelGrid;
typedef pcl::StatisticalOutlierRemoval<pcl::PointXYZ> StatisticalOutlierRemoval;
typedef pcl::SACSegmentation<pcl::PointXYZ> SACSegmentation;
typedef pcl::ExtractIndices<pcl::PointXYZ> ExtractIndices;
typedef pcl::ProjectInliers<pcl::PointXYZ> ProjectInliers;
typedef pcl::ModelCoefficients ModelCoefficients;
typedef pcl::ModelCoefficients::Ptr ModelCoefficientsPtr;
typedef pcl::PointIndices PointIndices;
typedef pcl::PointIndices::Ptr PointIndicesPtr;
typedef pcl::ConcaveHull<pcl::PointXYZ> ConcaveHull;
typedef pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> PointCloudColorHandlerCustom;
public:
boost::mutex cloud_mutex;
PCLVisualizer viewer;
PassThrough XpassFilter, YpassFilter, ZpassFilter;
MovingLeastSquares smoothedFilter;
VoxelGrid voxelFilter;
StatisticalOutlierRemoval statisFilter;
SACSegmentation segPlane;
ExtractIndices extractPlane;
ProjectInliers project;
VolumDetectViewer();
void cloud_cb_(ConstPointCloudConstPtr& cloud, PointCloudPtr& Prev_cloud, bool* new_cloud_available);
void run();
static PointCloudPtr smooth_cloud(PointCloudPtr& CloudPtr);
static PointCloudPtr extract_targets(PointCloudPtr& CloudPtr, PointIndicesPtr inliersPlane, ExtractIndices& extractPlane);
static bool extract_pointcloud(PointCloudPtr& CloudPtr, SACSegmentation& segPlane, ModelCoefficientsPtr coefficients, PointIndicesPtr inliers);
static void EstimateNormal(ConstPointCloudPtr cloud, NormalPtr normals);
static void EstimateBoundary(PointCloudPtr cloud, NormalPtr normals, Boundary& boundaries);
static void CheckBoundary(PointCloudPtr cloud_boundary, PointCloudPtr target_projected, Boundary& boundaries);
};
#include "volDetectVisualizer.h"
void VolumDetectViewer::EstimateNormal(ConstPointCloudPtr cloud, NormalPtr normals)
{
NormalEstimation normal_est;
normal_est.setInputCloud(cloud);
normal_est.setKSearch(100);
pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
normal_est.setSearchMethod(kdtree);
normal_est.compute(*normals);
}
void VolumDetectViewer::EstimateBoundary(PointCloudPtr cloud, NormalPtr normals, Boundary& boundaries) {
BoundaryEstimation boundary_est;
boundary_est.setInputCloud(cloud);
boundary_est.setInputNormals(normals);
boundary_est.setKSearch(100);
boundary_est.setAngleThreshold(M_PI / 2);
pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
boundary_est.setSearchMethod(kdtree);
boundary_est.compute(boundaries);
}
void VolumDetectViewer::CheckBoundary(PointCloudPtr cloud_boundary, PointCloudPtr target_projected, Boundary& boundaries)
{
std::vector<float> contour_x;
std::vector<float> contour_y;
for (size_t i = 0; i < target_projected->points.size(); ++i)
{
if (boundaries[i].boundary_point > 0)
{
cloud_boundary->push_back(target_projected->points[i]);
contour_x.push_back(target_projected->points[i].x);
contour_y.push_back(target_projected->points[i].y);
}
}
std::sort(contour_x.begin(), contour_x.end());
size_t n = contour_x.size();
if (abs(contour_x[n - 1] - 0.18f) <= 0.03f || abs(contour_x[0] + 0.18f) <= 0.03f)
std::cout << "物体在横向越界" << std::endl;
std::sort(contour_y.begin(), contour_y.end());
n = contour_y.size();
if (abs(contour_y[n - 1] - 0.15f) <= 0.03f || abs(contour_y[0] + 0.15f) <= 0.03f)
std::cout << "物体在纵向越界" << std::endl;
}
VolumDetectViewer::VolumDetectViewer() : viewer("PCLVIEW")
{
XpassFilter.setFilterFieldName("x");
XpassFilter.setFilterLimits(-0.18, 0.18);
YpassFilter.setFilterFieldName("y");
YpassFilter.setFilterLimits(-0.15, 0.15);
ZpassFilter.setFilterFieldName("z");
ZpassFilter.setFilterLimits(0.6f, 1.2f);
voxelFilter.setLeafSize(0.008f, 0.008f, 0.008f);
statisFilter.setMeanK(50);
statisFilter.setStddevMulThresh(1.0);
segPlane.setOptimizeCoefficients(true);
segPlane.setModelType(pcl::SACMODEL_PLANE);
segPlane.setMethodType(pcl::SAC_RANSAC);
segPlane.setDistanceThreshold(0.01);
segPlane.setMaxIterations(1000);
project.setModelType(pcl::SACMODEL_PLANE);
}
void VolumDetectViewer::cloud_cb_(ConstPointCloudConstPtr& cloud, PointCloudPtr& Prev_cloud, bool* new_cloud_available)
{
cloud_mutex.lock();
*Prev_cloud = *cloud;
*new_cloud_available = true;
cloud_mutex.unlock();
}
VolumDetectViewer::PointCloudPtr VolumDetectViewer::smooth_cloud(PointCloudPtr& cloudPtr)
{
KdTreePtr treeSampling(new KdTree);
PointCloud mls_point;
MovingLeastSquares mls;
mls.setComputeNormals(false);
mls.setInputCloud(cloudPtr);
mls.setPolynomialOrder(2);
mls.setSearchMethod(treeSampling);
mls.setSearchRadius(0.01);
mls.process(mls_point);
return mls_point.makeShared();
}
VolumDetectViewer::PointCloudPtr VolumDetectViewer::extract_targets(PointCloudPtr& CloudPtr, PointIndicesPtr inliersPlane, ExtractIndices& extractPlane)
{
PointCloud extracted;
extractPlane.setInputCloud(CloudPtr);
extractPlane.setIndices(inliersPlane);
extractPlane.setNegative(true);
extractPlane.filter(extracted);
return extracted.makeShared();
}
bool VolumDetectViewer::extract_pointcloud(PointCloudPtr& CloudPtr, SACSegmentation& segPlane, ModelCoefficientsPtr coefficients, PointIndicesPtr inliers)
{
segPlane.setInputCloud(CloudPtr);
segPlane.segment(*inliers, *coefficients);
return (inliers->indices.size() != 0);
}
void VolumDetectViewer::run()
{
pcl::Grabber* interface = new pcl::io::OpenNI2Grabber();
PointCloudPtr cloud(new PointCloud);
bool new_cloud_available = false;
std::function<void(ConstPointCloudConstPtr&)> f =
boost::bind(&VolumDetectViewer::cloud_cb_, this, boost::placeholders::_1, cloud, &new_cloud_available);
interface->registerCallback(f);
interface->start();
while (!viewer.wasStopped())
{
if (new_cloud_available && cloud_mutex.try_lock())
{
new_cloud_available = false;
PointCloudPtr pass_filtered(new pcl::PointCloud<pcl::PointXYZ>);
XpassFilter.setInputCloud(cloud);
XpassFilter.filter(*pass_filtered);
YpassFilter.setInputCloud(pass_filtered);
YpassFilter.filter(*pass_filtered);
ZpassFilter.setInputCloud(pass_filtered);
ZpassFilter.filter(*pass_filtered);
PointCloudPtr voxel_filtered(new pcl::PointCloud<pcl::PointXYZ>);
voxelFilter.setInputCloud(pass_filtered);
voxelFilter.filter(*voxel_filtered);
PointCloudPtr statis_Filtered(new pcl::PointCloud<pcl::PointXYZ>);
statisFilter.setInputCloud(voxel_filtered);
statisFilter.filter(*statis_Filtered);
auto cloud_smoothed = smooth_cloud(statis_Filtered);
ModelCoefficientsPtr coefficientsPlane(new ModelCoefficients());
PointIndicesPtr inliersPlane(new PointIndices());
if (extract_pointcloud(cloud_smoothed, segPlane, coefficientsPlane, inliersPlane))
{
PointCloudPtr PlaneCloud(new PointCloud);
pcl::copyPointCloud(*cloud_smoothed, inliersPlane->indices, *PlaneCloud);
PointCloudColorHandlerCustom Plane_color(PlaneCloud, 139, 139, 0);
auto extracted = extract_targets(cloud_smoothed, inliersPlane, extractPlane);
ModelCoefficientsPtr coefficientsTarget(new ModelCoefficients());
PointIndicesPtr inliersTarget(new PointIndices());
if (extract_pointcloud(extracted, segPlane, coefficientsTarget, inliersTarget))
{
PointCloudPtr TargetCloud(new PointCloud);
pcl::copyPointCloud(*extracted, inliersTarget->indices, *TargetCloud);
PointCloudColorHandlerCustom Target_color(TargetCloud, 205, 201, 201);
float height = abs(coefficientsPlane->values[3] / coefficientsPlane->values[2] - coefficientsTarget->values[3] / coefficientsPlane->values[2]);
PointCloudPtr target_projected(new PointCloud);
project.setIndices(inliersTarget);
project.setInputCloud(extracted);
project.setModelCoefficients(coefficientsTarget);
project.filter(*target_projected);
PointCloudColorHandlerCustom target_projected_color(target_projected, 255, 69, 0);
NormalPtr normals(new Normal);
EstimateNormal(target_projected, normals);
Boundary boundaries;
EstimateBoundary(target_projected, normals, boundaries);
PointCloudPtr cloud_boundary(new pcl::PointCloud<pcl::PointXYZ>);
CheckBoundary(cloud_boundary, target_projected, boundaries);
PointCloudColorHandlerCustom boundary_color(cloud, 0, 255, 0);
PointCloudPtr cloud_hull(new PointCloud);
ConcaveHull chull;
vector<pcl::Vertices> polygons;
chull.setInputCloud(target_projected);
chull.setAlpha(0.1);
chull.reconstruct(*cloud_hull, polygons);
PointCloudColorHandlerCustom cloud_hull_color(cloud_hull, 0, 245, 255);
vector<Point2f> hullContour;
for (int j = 0; j < polygons[0].vertices.size(); ++j)
{
hullContour.push_back(Point2f(cloud_hull->points[polygons[0].vertices[j]].x,
cloud_hull->points[polygons[0].vertices[j]].y));
}
RotatedRect minRect = cv::minAreaRect(hullContour);
float realWidth = 1.145 * 100 * std::max(minRect.size.width, minRect.size.height);
float realHeight = 1.145 * 100 * std::min(minRect.size.width, minRect.size.height);
cout << setiosflags(ios::fixed) << setprecision(2);
std::cout << "length:" << realWidth << " width:" << realHeight << " height " << height * 100 << std::endl;
viewer.removeAllPointClouds();
viewer.removeAllShapes();
viewer.addPointCloud<pcl::PointXYZ>(statis_Filtered, "cloud");
viewer.addPointCloud<pcl::PointXYZ>(cloud_smoothed, "cloud_smoothed");
viewer.addPointCloud<pcl::PointXYZ>(PlaneCloud, Plane_color, "Plane");
viewer.addPointCloud<pcl::PointXYZ>(TargetCloud, Target_color, "Target");
viewer.addPointCloud<pcl::PointXYZ>(target_projected, target_projected_color, "target_projected");
viewer.addPointCloud<pcl::PointXYZ>(cloud_boundary, boundary_color, "boundaries");
viewer.addPointCloud<pcl::PointXYZ>(cloud_hull, cloud_hull_color, "cloud_hull");
}
}
viewer.spinOnce();
cloud_mutex.unlock();
}
}
interface->stop();
}
int main()
{
VolumDetectViewer v;
v.run();
return (0);
}