C++ PointCloud::isOrganized方法代码示例

void AddGroundTruth::setInputCloud(pcl::PointCloud<pcl::PointXYZRGBL>::Ptr _cloud)
{
  if ( (_cloud->height <= 1) || (_cloud->width <= 1) || (!_cloud->isOrganized()) )
    throw std::runtime_error("[AddGroundTruth::setInputCloud] Invalid point cloud (height must be > 1)");

  //@ep: check type here!!!
  
  cloud = _cloud;
  width = cloud->width;
  height = cloud->height;

  have_cloud = true;
  have_surfaces = false;
  have_relations = false;
  computed = false;
}

void OpenniFilter::cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud)
{
    if (!viewer.wasStopped())
    {
        if (cloud->isOrganized())
        {
            // initialize all the Mats to store intermediate steps
            int cloudHeight = cloud->height;
            int cloudWidth = cloud->width;
            rgbFrame = Mat(cloudHeight, cloudWidth, CV_8UC3);
            drawing = Mat(cloudHeight, cloudWidth, CV_8UC3, NULL);
            grayFrame = Mat(cloudHeight, cloudWidth, CV_8UC1, NULL);
            hsvFrame = Mat(cloudHeight, cloudWidth, CV_8UC3, NULL);
            contourMask = Mat(cloudHeight, cloudWidth, CV_8UC1, NULL);

            if (!cloud->empty())
            {
                for (int h = 0; h < rgbFrame.rows; h ++)
                {
                    for (int w = 0; w < rgbFrame.cols; w++)
                    {
                        pcl::PointXYZRGBA point = cloud->at(w, cloudHeight-h-1);
                        Eigen::Vector3i rgb = point.getRGBVector3i();
                        rgbFrame.at<Vec3b>(h,w)[0] = rgb[2];
                        rgbFrame.at<Vec3b>(h,w)[1] = rgb[1];
                        rgbFrame.at<Vec3b>(h,w)[2] = rgb[0];
                    }
                }

                // do the filtering 
                int xPos = 0;
                int yPos = 0;
                mtx.lock();
                xPos = mouse_x;
                yPos = mouse_y;
                mtx.unlock();

                // color filtering based on what is chosen by users
                cvtColor(rgbFrame, hsvFrame, CV_RGB2HSV);
                Vec3b pixel = hsvFrame.at<Vec3b>(xPos,yPos);

                int hueLow = pixel[0] < iHueDev ? pixel[0] : pixel[0] - iHueDev;
                int hueHigh = pixel[0] > 255 - iHueDev ? pixel[0] : pixel[0] + iHueDev;
                // inRange(hsvFrame, Scalar(hueLow, pixel[1]-20, pixel[2]-20), Scalar(hueHigh, pixel[1]+20, pixel[2]+20), grayFrame);
                inRange(hsvFrame, Scalar(hueLow, iLowS, iLowV), Scalar(hueHigh, iHighS, iHighV), grayFrame);

                // removes small objects from the foreground by morphological opening
                erode(grayFrame, grayFrame, getStructuringElement(MORPH_ELLIPSE, Size(5,5)));
                dilate(grayFrame, grayFrame, getStructuringElement(MORPH_ELLIPSE, Size(5,5)));

                // morphological closing (removes small holes from the foreground)
                dilate(grayFrame, grayFrame, getStructuringElement(MORPH_ELLIPSE, Size(5,5)));
                erode(grayFrame, grayFrame, getStructuringElement(MORPH_ELLIPSE, Size(5,5)));

                // gets contour from the grayFrame and keeps the largest contour
                Mat cannyOutput;
                vector<vector<Point> > contours;
                vector<Vec4i> hierarchy;
                int thresh = 100;
                Canny(grayFrame, cannyOutput, thresh, thresh * 2, 3);
                findContours(cannyOutput, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, Point(0, 0));
                int largestContourArea, largestContourIndex = 0;
                int defaultContourArea = 1000; // 1000 seems to work find in most cases... cannot prove this
                vector<vector<Point> > newContours;
                for (int i = 0; i < contours.size(); i++)
                {
                    double area = contourArea(contours[i], false);
                    if (area > defaultContourArea)
                        newContours.push_back(contours[i]);
                }

                // draws the largest contour: 
                drawing = Mat::zeros(cannyOutput.size(), CV_8UC3);
                for (int i = 0; i < newContours.size(); i++)
                    drawContours(drawing, newContours, i, Scalar(255, 255, 255), CV_FILLED, 8, hierarchy, 0, Point());

                // gets the filter by setting everything within the contour to be 1. 
                inRange(drawing, Scalar(1, 1, 1), Scalar(255, 255, 255), contourMask);

                // filters the point cloud based on contourMask
                // again go through the point cloud and filter out unnecessary points
                pcl::PointCloud<pcl::PointXYZRGBA>::Ptr resultCloud (new pcl::PointCloud<pcl::PointXYZRGBA>);
                pcl::PointXYZRGBA newPoint;
                for (int h = 0; h < contourMask.rows; h ++)
                {
                    for (int w = 0; w < contourMask.cols; w++)
                    {
                        if (contourMask.at<uchar>(h,w) > 0)
                        {
                            newPoint = cloud->at(w,h);
                            resultCloud->push_back(newPoint);
                        }
                    }
                }

                if (xPos == 0 && yPos == 0)
                    viewer.showCloud(cloud);
                else
                    viewer.showCloud(resultCloud);
                
//.........这里部分代码省略.........