cv_func.h
2.53 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
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
109
110
111
112
113
#pragma once
#include "common.h"
void readFolder(std::string folderPath, std::vector<cv::Mat>& depths)
{
path p(folderPath);
try
{
if(exists(p) && is_directory(p))
{
std::vector<std::string> imgs;
for(directory_entry& x : directory_iterator(p))
{
std::string img = x.path().string();
imgs.push_back(img);
}
std::sort(imgs.begin(), imgs.end());
for(auto img : imgs)
{
cv::Mat depth = cv::imread(img, cv::IMREAD_ANYDEPTH); //16UC1
depths.push_back(depth);
}
}
else
{
std::cout << "[Error] Check Depth Map Folder Path" << std::endl;
exit(0);
}
}
catch(const filesystem_error& ex)
{
std::cout << ex.what() << std::endl;
}
}
std::string type2str(int type)
{
std::string r;
uchar depth = type & CV_MAT_DEPTH_MASK;
uchar chans = 1 + (type >> CV_CN_SHIFT);
switch ( depth )
{
case CV_8U: r = "8U"; break;
case CV_8S: r = "8S"; break;
case CV_16U: r = "16U"; break;
case CV_16S: r = "16S"; break;
case CV_32S: r = "32S"; break;
case CV_32F: r = "32F"; break;
case CV_64F: r = "64F"; break;
default: r = "User"; break;
}
r += "C";
r += (chans+'0');
return r;
}
cv::Mat getMeanImg(std::vector<cv::Mat> depths)
{
if(depths.empty())
{
std::cout << "[Error] no input images" << std::endl;
exit(0);
}
cv::Mat m(depths[0].rows, depths[0].cols, CV_64FC1);
m.setTo(0);
cv::Mat tmp;
for(auto depth : depths)
{
depth.convertTo(tmp, CV_64FC1);
m += tmp;
}
m.convertTo(m, CV_16UC1, 1.0 / depths.size());
return m;
}
cv::Mat medianFilter(cv::Mat img)
{
cv::Mat filtered;
cv::medianBlur(img, filtered, 5);
return filtered;
}
void reconstruction3D(camInfo cam, cv::Mat img, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
for(int i = 0; i < img.cols; i++)
{
for(int j = 0; j < img.rows; j++)
{
auto d = img.at<u_int16_t>(j, i) / 1000.0;
if(d < 0.105 || d > 3.0)
{ //realsense min dist && max dist
continue;
}
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);
}
}
}