densedepth.cpp
1.59 KB
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
#include "common.h"
#include "cv_func.h"
#include "pcl_func.h"
/*
[input]
argc = 2
argv[0] = program
argv[1] = densedepth.png (320 x 240)
[output]
interpolated_densedepth.png (640 x 480)
densedepth.pcd
*/
int main(int argc, char** argv)
{
if(argc != 2)
{
std::cout << "[Error] Program Usage: [./densedepth] [densedepth.png]" << std::endl;
return -1;
}
//intrinsic setup
camInfo cam;
realsenseSetUp(cam);
cv::Mat img, interpolated_img;
img = cv::imread(argv[1], cv::IMREAD_ANYDEPTH);
//std::cout << type2str(img.type()) << std::endl; -> 16UC1
//interpolation to (320, 240) -> (640, 480)
cv::resize(img, interpolated_img, cv::Size(640, 480), cv::INTER_LINEAR);
std::string inputPath = argv[1];
std::size_t div = inputPath.find_last_of("/\\");
std::string outputPath = inputPath.substr(0, div + 1);
cv::imwrite(outputPath + "interpolated_densedepth.png", interpolated_img);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
for(int i = 0; i < interpolated_img.cols; i++)
{
for(int j = 0; j < interpolated_img.rows; j++)
{
auto d = (-interpolated_img.at<u_int16_t>(j, i)) / 1000.0; // closer - darker reversed
double x = (i - cam.cx) * d / cam.fx;
double y = (j - cam.cy) * d / cam.fy;
double z = d;
pcl::PointXYZ pt;
pt.x = x;
pt.y = y;
pt.z = z;
cloud->push_back(pt);
}
}
pcl::io::savePCDFile(outputPath + "densedepth.pcd", *cloud);
return 0;
}