-
Notifications
You must be signed in to change notification settings - Fork 75
/
StereoMatching.cpp
155 lines (141 loc) · 4.99 KB
/
StereoMatching.cpp
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
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
/**
@file StereoMatcing.cpp
@date 2017/09/03
@author tkwoo([email protected]).
@brief create disparity map
*/
#include "StereoMatching.h"
// Ptr<StereoBM> bm = StereoBM::create(48, 9); ///< default construct
CStereoMatching::CStereoMatching(StereoCamParam_t& objStereoParam)
{
SetParamOCVStereo(objStereoParam);
}
CStereoMatching::CStereoMatching(Mat& imgLeftInput, Mat& imgRightInput, StereoCamParam_t& objStereoParam)
{
SetParamOCVStereo(objStereoParam);
m_imgLeftInput = imgLeftInput;
m_imgRightInput = imgRightInput;
}
void CStereoMatching::SetParamOCVStereo(StereoCamParam_t& objStereoParam)
{
m_objStereoParam = objStereoParam;
#if CV_MAJOR_VERSION==3
bm->create(m_objStereoParam.m_nNumberOfDisp, m_objStereoParam.m_nWindowSize);
bm->setPreFilterCap(31);
bm->setBlockSize(m_objStereoParam.m_nWindowSize > 0 ? m_objStereoParam.m_nWindowSize : 9);
bm->setMinDisparity(0);
bm->setNumDisparities(m_objStereoParam.m_nNumberOfDisp);
bm->setTextureThreshold(10); /// SAD window response threshold : default=12
bm->setUniquenessRatio(5); /// >(match_val - min_match)/min_match
bm->setSpeckleWindowSize(100); //25;//9;
bm->setSpeckleRange(32); //4;
//bm->setSmallerBlockSize(9);
//bm->setDisp12MaxDiff(1);
#else
bm.state->preFilterCap = 31;
bm.state->SADWindowSize = m_objStereoParam.m_nWindowSize > 0 ? m_objStereoParam.m_nWindowSize : 9;
bm.state->minDisparity = 0;
bm.state->numberOfDisparities = m_objStereoParam.m_nNumberOfDisp;
bm.state->textureThreshold = 10; //SAD window response threshold : default=12
bm.state->uniquenessRatio = 15; // > (match_val - min_match)/min_match
bm.state->speckleWindowSize = 100;//25;//9;
bm.state->speckleRange = 32;//4;
//bm.state->disp12MaxDiff = 1;
#endif
}
MATCHING_ERROR CStereoMatching::SetImage(Mat& imgLeft, Mat& imgRight){
if (imgLeft.size() != imgRight.size()) return IMGSCALE_ERR;
if (imgLeft.size() != m_objStereoParam.objCamParam.m_sizeSrc) return IMGSCALE_ERR;
if (imgLeft.channels() == 3){
cvtColor(imgLeft, m_imgLeftInput, CV_BGR2GRAY);
cvtColor(imgRight, m_imgRightInput, CV_BGR2GRAY);
return NO_PROB;
}
m_imgLeftInput = imgLeft;
m_imgRightInput = imgRight;
return NO_PROB;
}
MATCHING_ERROR CStereoMatching::MakeDisparity()
{
MakeDisparity(m_imgLeftInput, m_imgRightInput, m_matDisp16);
m_matDisp16.convertTo(m_imgDisp8, CV_8U, 255 / (m_objStereoParam.m_nNumberOfDisp*16.));
//m_imgDisp8_ori = m_imgDisp8.clone();
//imshow("disp", m_imgDisp8);
return NO_PROB;
}
MATCHING_ERROR CStereoMatching::MakeDisparity(Mat& imgLeft, Mat& imgRight, bool flgUseWLSFilter)
{
#if CV_MAJOR_VERSION==2
flgUseWLSFilter = false;
#endif
MATCHING_ERROR Error;
Error = SetImage(imgLeft, imgRight);
//cout << Error << endl;
if (flgUseWLSFilter == false){
MakeDisparity();
ImproveDisparity_Naive(m_imgDisp8);
}
#if CV_MAJOR_VERSION==3
else if (flgUseWLSFilter == false){
}
// There is matching code inside the WLSFilter(), It is
else ImproveDisparity_WLSFilter(m_imgDisp8);
#endif
return Error;
}
MATCHING_ERROR CStereoMatching::MakeDisparity(Mat& imgLeft, Mat& imgRight, Mat& matDisp16){
#if CV_MAJOR_VERSION==3
// cout << "SADWindowSize : " << bm->getBlockSize() << endl;
// cout << "NumOfDisparity : " << bm->getNumDisparities() << endl;
bm->compute(imgLeft, imgRight, matDisp16);
// cout << "complete" << endl;
#else
bm(imgLeft, imgRight, matDisp16, CV_16S);
#endif
m_matDisp16 = matDisp16;
return NO_PROB;
}
MATCHING_ERROR CStereoMatching::ImproveDisparity_Naive(Mat& imgDisp8)
{
uchar chTempCur = 0;
uchar chTempPrev = 0;
int cnt = 1;
for (int v = 0; v < imgDisp8.rows; v++){
for (int u = m_objStereoParam.m_nNumberOfDisp; u < imgDisp8.cols; u++){
chTempCur = imgDisp8.at<uchar>(v, u);
//shTempCur = m_matDisp16.at<short>(v, u);
if (chTempCur == 0) {
imgDisp8.at<uchar>(v, u) = chTempPrev;
//m_matDisp16.at<short>(v, u) = shTempPrev;
}
else {
chTempPrev = chTempCur;
//shTempPrev = shTempCur;
}
}
}
Size size(7, 7);
Mat matKernel = getStructuringElement(MORPH_RECT, size);
morphologyEx(imgDisp8, imgDisp8, MORPH_OPEN, matKernel, Point(-1, -1), 2);
return NO_PROB;
}
MATCHING_ERROR CStereoMatching::ImproveDisparity_WLSFilter(Mat& imgDisp8)
{
#if CV_MAJOR_VERSION==3
Mat matDispLeft16;
Mat matDispRight16;
/*Mat conf_map = Mat(m_imgLeftInput.rows, m_imgLeftInput.cols, CV_8U);
conf_map = Scalar(255);*/
wls_filter = createDisparityWLSFilter(bm);
Ptr<StereoMatcher> right_bm = createRightMatcher(bm);
wls_filter->setLambda(8000.);
wls_filter->setSigmaColor(1.5);
bm->compute(m_imgLeftInput, m_imgRightInput, matDispLeft16);
right_bm->compute(m_imgRightInput, m_imgLeftInput, matDispRight16);
wls_filter->filter(matDispLeft16, m_imgLeftInput, m_matDisp16, matDispRight16);
/*conf_map = wls_filter->getConfidenceMap();
m_rectFilterROI = wls_filter->getROI();*/
m_matDisp16.convertTo(imgDisp8, CV_8U, 255 / (m_objStereoParam.m_nNumberOfDisp*16.));
#endif
return NO_PROB;
}