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
|
/**
* attention: Astra2 cameras currently only support Windows and Linux kernel versions no higher than 4.15, and higher versions of Linux kernel may have exceptions.
*/
#include <opencv2/videoio.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <iostream>
using namespace cv;
int main()
{
VideoCapture obsensorCapture(0, CAP_OBSENSOR);
if(!obsensorCapture.isOpened()){
std::cerr << "Failed to open obsensor capture! Index out of range or no response from device";
return -1;
}
double fx = obsensorCapture.get(CAP_PROP_OBSENSOR_INTRINSIC_FX);
double fy = obsensorCapture.get(CAP_PROP_OBSENSOR_INTRINSIC_FY);
double cx = obsensorCapture.get(CAP_PROP_OBSENSOR_INTRINSIC_CX);
double cy = obsensorCapture.get(CAP_PROP_OBSENSOR_INTRINSIC_CY);
std::cout << "obsensor camera intrinsic params: fx=" << fx << ", fy=" << fy << ", cx=" << cx << ", cy=" << cy << std::endl;
Mat image;
Mat depthMap;
Mat adjDepthMap;
// Minimum depth value
const double minVal = 300;
// Maximum depth value
const double maxVal = 5000;
while (true)
{
// Grab depth map like this:
// obsensorCapture >> depthMap;
// Another way to grab depth map (and bgr image).
if (obsensorCapture.grab())
{
if (obsensorCapture.retrieve(image, CAP_OBSENSOR_BGR_IMAGE))
{
imshow("RGB", image);
}
if (obsensorCapture.retrieve(depthMap, CAP_OBSENSOR_DEPTH_MAP))
{
depthMap.convertTo(adjDepthMap, CV_8U, 255.0 / (maxVal - minVal), -minVal * 255.0 / (maxVal - minVal));
applyColorMap(adjDepthMap, adjDepthMap, COLORMAP_JET);
imshow("DEPTH", adjDepthMap);
}
// depth map overlay on bgr image
static const float alpha = 0.6f;
if (!image.empty() && !depthMap.empty())
{
depthMap.convertTo(adjDepthMap, CV_8U, 255.0 / (maxVal - minVal), -minVal * 255.0 / (maxVal - minVal));
cv::resize(adjDepthMap, adjDepthMap, cv::Size(image.cols, image.rows));
for (int i = 0; i < image.rows; i++)
{
for (int j = 0; j < image.cols; j++)
{
cv::Vec3b& outRgb = image.at<cv::Vec3b>(i, j);
uint8_t depthValue = 255 - adjDepthMap.at<uint8_t>(i, j);
if (depthValue != 0 && depthValue != 255)
{
outRgb[0] = (uint8_t)(outRgb[0] * (1.0f - alpha) + depthValue * alpha);
outRgb[1] = (uint8_t)(outRgb[1] * (1.0f - alpha) + depthValue * alpha);
outRgb[2] = (uint8_t)(outRgb[2] * (1.0f - alpha) + depthValue * alpha);
}
}
}
imshow("DepthToColor", image);
}
image.release();
depthMap.release();
}
if (pollKey() >= 0)
break;
}
return 0;
}
|
