kinectcap/pcd2csv.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
// std
#include <string>
#include <iostream>

// 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::PointCloud<PointT> PC;
const static Eigen::IOFormat CSVFormat(Eigen::StreamPrecision, Eigen::DontAlignCols, ",", "\n");

int writeMat2File(const Eigen::MatrixXf &matrix, const std::string &fileName);


int
writeMat2File(const Eigen::MatrixXf &matrix, const std::string &fileName)
{
  std::ofstream out( fileName.c_str() );

  if (out.is_open())
    out << matrix.format(CSVFormat);
  else
    return 0;

  out.close();
  return 1;
}


int main(int argc, char ** argv)
{
    std::string fNamePCD = "";
    std::string fNamePIC = "";

    // Load point cloud from file
    std::string fName = "";
    if(argc > 2)
    {
        fNamePCD = std::string(argv[1]);
        fNamePIC = std::string(argv[2]);
        if(fNamePCD == "-h")
        {
            std::cout << "Usage: pcd2csv pcd-FileName pic-FileName" << std::endl;
            return -1;
        }
    }
    else
    {
        std::cout << "Please specify a pcd file and its corresponding image to convert to csv." << std::endl;
        std::cout << "Usage: pcd2csv pcd-FileName pic-FileName" << std::endl;
        return -1;
    }


    // READ PCD AND IM FILES
    cv::Mat im;
    im = cv::imread(fNamePIC);

    pcl::PCDReader reader;
    PC::Ptr cloud (new PC);
    reader.read(fNamePCD, *cloud);

    int i = 640*480;
    Eigen::MatrixXf X(307200,6);

    for(int iRow=0; iRow!=480; iRow++)
    {
        for(int iCol=0; iCol!=640; iCol++)
        {
            int idx = iRow*640 + iCol;
            // // img x-y-pos
            // X(idx,0) = iRow;
            // X(idx,1) = iCol;
            // img
            X(idx,0) = im.at<cv::Vec3b>(iRow,iCol)[0];
            X(idx,1) = im.at<cv::Vec3b>(iRow,iCol)[1];
            X(idx,2) = im.at<cv::Vec3b>(iRow,iCol)[2];
            // img
            // PointT p_valid = cloudIn->at(idx);
            X(idx,3) = cloud->at(idx).x;
            X(idx,4) = cloud->at(idx).y;
            X(idx,5) = cloud->at(idx).z;
        }
    }

    int fileNameLen = fNamePCD.length();

    writeMat2File(X, fNamePCD.substr(0,fileNameLen-4)+".csv");

    return 1;
}