kinectcap/pcd2pic.cpp at master · MartinHjelm/kinectcap · 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
106
107
108
// std
#include <string>
#include <iostream>

// boost
#include <boost/program_options.hpp>

// Opencv
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

// pcl
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/filters/statistical_outlier_removal.h>

typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointXYZ PointT2;
typedef pcl::PointCloud<PointT> PC;
typedef pcl::PointCloud<PointT2> PC2;


int main(int argc, char ** argv)
{

    // Load point cloud from file
    std::string fNamePCD = "";
    if(argv[1])
    {
        fNamePCD = std::string(argv[1]);
        if(fNamePCD == "-h")
        {
            std::cout << "Usage: pcd2pic fileName xyz. xyz argument used if you want to remove color." << std::endl;
            return -1;
        }
    }
    else
    {
        std::cout << "Please specify a file to convert." << std::endl;
        std::cout << "Usage: pcd2pic fileName xyz. xyz argument used if you want to remove color." << std::endl;
        return -1;
    }


    bool pcTypeRGB = true;

    if(argc>2)
    {
        if(std::string(argv[2])=="xyz") // Get only xyz no color data
            pcTypeRGB = false;
    }

    pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
    pcl::PointCloud<PointT2>::Ptr cloud2 (new pcl::PointCloud<PointT2>);

    pcl::PCDReader reader;
    if(pcTypeRGB)
        reader.read (fNamePCD, *cloud);
    else
        reader.read (fNamePCD, *cloud2);

    cv::Mat pcImg = cv::Mat::zeros(480,640,CV_8UC3);
    double focalInv = 1000.0/525.0;


    if (pcTypeRGB)
    {

        // Make whole image color of blue so that we can see lost pixels
        pcImg.setTo(cv::Scalar(255,0,0));

        std::vector<int> indices;
        pcl::removeNaNFromPointCloud(*cloud,*cloud, indices);
        for(PC::const_iterator iter=cloud->begin(); iter!=cloud->end(); ++iter)
        {
            double xm = iter->x; double ym = iter->y; double zm = iter->z;
            int xPos = (1000.0 * xm / (zm * focalInv)) + 320;
            int yPos = (1000.0 * ym / (zm * focalInv)) + 240;

            pcImg.at<cv::Vec3b>(yPos,xPos)[0] = iter->b; //BGR 2 RGB
            pcImg.at<cv::Vec3b>(yPos,xPos)[1] = iter->g;
            pcImg.at<cv::Vec3b>(yPos,xPos)[2] = iter->r;
        }
    }
    else
    {
        std::vector<int> indices;
        pcl::removeNaNFromPointCloud(*cloud2,*cloud2, indices);
        for(PC2::const_iterator iter=cloud2->begin(); iter!=cloud2->end(); ++iter)
        {
            double xm = iter->x; double ym = iter->y; double zm = iter->z;
            int xPos = (1000.0 * xm / (zm * focalInv)) + 320;
            int yPos = (1000.0 * ym / (zm * focalInv)) + 240;

            pcImg.at<cv::Vec3b>(yPos,xPos)[0] = 255; //BGR 2 RGB
            pcImg.at<cv::Vec3b>(yPos,xPos)[1] = 255;
            pcImg.at<cv::Vec3b>(yPos,xPos)[2] = 255;
        }
    }

    int fileNameLen = fNamePCD.length();
    cv::imwrite(fNamePCD.substr(0,fileNameLen-4)+"_pcd.png", pcImg);
    return 1;
}