KinectToPCL/PointCloudUtilities.cpp at master · gjc13/KinectToPCL · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
//
// Created by 郭嘉丞 on 15/9/21.
//

#include "PointcloudUtilities.h"
#include <pcl/visualization/pcl_visualizer.h>
#include <cstdio>

//reload depth image
cv::Mat reload_32f_image(const char * filename)
{
    fstream fin(filename);
    int num_rows, num_cols;
    fin.read((char *) &num_rows, sizeof(int));
    fin.read((char *) &num_cols, sizeof(int));
    cv::Mat mat = cv::Mat::zeros(num_rows, num_cols, CV_32FC1);
    fin.read((char *) mat.data, num_cols * num_rows * 4);
    return mat;
}

void rgbVis(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud)
{
    // --------------------------------------------
    // -----Open 3D viewer and add point cloud-----
    // --------------------------------------------
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
    viewer->setBackgroundColor(0, 0, 0);
    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
    viewer->addPointCloud<pcl::PointXYZRGB>(cloud, rgb, "sample cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
    viewer->addCoordinateSystem(1.0);
    viewer->initCameraParameters();
    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    }
}

PointCloudPtr getSubXCloud(PointCloudPtr cloud, double fromX, double toX)
{
    PointCloudPtr newCloud(new pcl::PointCloud<pcl::PointXYZRGB>());
    for (unsigned i=0; i<cloud->points.size(); i++)
    {
        const pcl::PointXYZRGB & point = cloud->points[i];
        if (point.x < toX && point.x > fromX)
        {
            newCloud->points.push_back(point);
        }
    }
    newCloud->width = (int) newCloud->points.size();
    newCloud->height = 1;
    return newCloud;
}

PointCloudPtr getSubYCloud(PointCloudPtr cloud, double fromY, double toY)
{
    PointCloudPtr newCloud(new pcl::PointCloud<pcl::PointXYZRGB>());
    for (unsigned i=0; i<cloud->points.size(); i++)
    {
        const pcl::PointXYZRGB & point = cloud->points[i];
        if (point.y < toY && point.y > fromY)
        {
            newCloud->points.push_back(point);
        }
    }
    newCloud->width = (int) newCloud->points.size();
    newCloud->height = 1;
    return newCloud;
}

PointCloudPtr getSubZCloud(PointCloudPtr cloud, double fromZ, double toZ)
{
    PointCloudPtr newCloud(new pcl::PointCloud<pcl::PointXYZRGB>());
    for (unsigned i=0; i<cloud->points.size(); i++)
    {
        const pcl::PointXYZRGB & point = cloud->points[i];
        if (point.z < toZ && point.z > fromZ)
        {
            newCloud->points.push_back(point);
        }
    }
    newCloud->width = (int) newCloud->points.size();
    newCloud->height = 1;
    return newCloud;
}

PointCloudPtr getSubCloud(PointCloudPtr cloud, double fromX, double toX,
                          double fromY, double toY, double fromZ, double toZ)
{
    PointCloudPtr newCloud(new pcl::PointCloud<pcl::PointXYZRGB>());
    for (unsigned i=0; i<cloud->points.size(); i++)
    {
        const pcl::PointXYZRGB & point = cloud->points[i];
        if (point.x < toX && point.x > fromX
            && point.y < toY && point.y > fromY
            && point.z < toZ && point.z > fromZ)
        {
            newCloud->points.push_back(point);
        }
    }
    newCloud->width = (int) newCloud->points.size();
    newCloud->height = 1;
    return newCloud;
}