Skip to content

Commit ceff777

Browse files
borongyuanborongyuan
committed
Add params to filter out poor stereo flow matches
1 parent 0ef9077 commit ceff777

File tree

5 files changed

+123
-10
lines changed

5 files changed

+123
-10
lines changed

‎corelib/include/rtabmap/core/Parameters.h‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -818,6 +818,9 @@ class RTABMAP_CORE_EXPORT Parameters
818818
RTABMAP_PARAM(Stereo, OpticalFlow, bool, true, "Use optical flow to find stereo correspondences, otherwise a simple block matching approach is used.");
819819
RTABMAP_PARAM(Stereo, SSD, bool, true, uFormat("[%s=false] Use Sum of Squared Differences (SSD) window, otherwise Sum of Absolute Differences (SAD) window is used.", kStereoOpticalFlow().c_str()));
820820
RTABMAP_PARAM(Stereo, Eps, double, 0.01, uFormat("[%s=true] Epsilon stop criterion.", kStereoOpticalFlow().c_str()));
821+
RTABMAP_PARAM(Stereo, GetMinEigenVals, bool, true, uFormat("[%s=true] Use minimum eigen values as an error measure, otherwise L1 distance between patches is used as an error measure.", kStereoOpticalFlow().c_str()));
822+
RTABMAP_PARAM(Stereo, MinEigThreshold, double, 1e-4, uFormat("[%s=true] If the minimum eigenvalue of a feature's spatial gradient matrix is less than minEigThreshold, then the feature is filtered out.", kStereoGetMinEigenVals().c_str()));
823+
RTABMAP_PARAM(Stereo, ErrorThreshold, double, 50, uFormat("[%s=false] Filter out features with error greater than errorThreshold.", kStereoGetMinEigenVals().c_str()));
821824
RTABMAP_PARAM(Stereo, Gpu, bool, false, uFormat("[%s=true] Enable GPU version of the optical flow approach (only available if OpenCV is built with CUDA).", kStereoOpticalFlow().c_str()));
822825

823826
RTABMAP_PARAM(Stereo, DenseStrategy, int, 0, "0=cv::StereoBM, 1=cv::StereoSGBM");

‎corelib/include/rtabmap/core/Stereo.h‎

Lines changed: 8 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -96,16 +96,23 @@ class RTABMAP_CORE_EXPORT StereoOpticalFlow : public Stereo {
9696
#endif
9797

9898
float epsilon() const {return epsilon_;}
99+
bool getMinEigenVals() const {return getMinEigenVals_;}
100+
float minEigThreshold() const {return minEigThreshold_;}
101+
float errorThreshold() const {return errorThreshold_;}
99102
virtual bool isGpuEnabled() const;
100103

101104
private:
102105
void updateStatus(
103106
const std::vector<cv::Point2f> & leftCorners,
104107
const std::vector<cv::Point2f> & rightCorners,
105-
std::vector<unsigned char> & status) const;
108+
std::vector<unsigned char> & status,
109+
std::vector<float> err = {}) const;
106110

107111
private:
108112
float epsilon_;
113+
bool getMinEigenVals_;
114+
float minEigThreshold_;
115+
float errorThreshold_;
109116
bool gpu_;
110117
};
111118

‎corelib/src/Stereo.cpp‎

Lines changed: 22 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -113,6 +113,9 @@ std::vector<cv::Point2f> Stereo::computeCorrespondences(
113113
StereoOpticalFlow::StereoOpticalFlow(const ParametersMap & parameters) :
114114
Stereo(parameters),
115115
epsilon_(Parameters::defaultStereoEps()),
116+
getMinEigenVals_(Parameters::defaultStereoGetMinEigenVals()),
117+
minEigThreshold_(Parameters::defaultStereoMinEigThreshold()),
118+
errorThreshold_(Parameters::defaultStereoErrorThreshold()),
116119
gpu_(Parameters::defaultStereoGpu())
117120
{
118121
this->parseParameters(parameters);
@@ -122,6 +125,9 @@ void StereoOpticalFlow::parseParameters(const ParametersMap & parameters)
122125
{
123126
Stereo::parseParameters(parameters);
124127
Parameters::parse(parameters, Parameters::kStereoEps(), epsilon_);
128+
Parameters::parse(parameters, Parameters::kStereoGetMinEigenVals(), getMinEigenVals_);
129+
Parameters::parse(parameters, Parameters::kStereoMinEigThreshold(), minEigThreshold_);
130+
Parameters::parse(parameters, Parameters::kStereoErrorThreshold(), errorThreshold_);
125131
Parameters::parse(parameters, Parameters::kStereoGpu(), gpu_);
126132
#ifndef HAVE_OPENCV_CUDAOPTFLOW
127133
if(gpu_)
@@ -171,11 +177,18 @@ std::vector<cv::Point2f> StereoOpticalFlow::computeCorrespondences(
171177
err,
172178
this->winSize(),
173179
this->maxLevel(),
174-
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, this->iterations(), epsilon_),
175-
cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
180+
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, this->iterations(), this->epsilon()),
181+
this->getMinEigenVals() ? cv::OPTFLOW_LK_GET_MIN_EIGENVALS : 0, this->minEigThreshold());
176182
UDEBUG("util2d::calcOpticalFlowPyrLKStereo() end");
177183
}
178-
updateStatus(leftCorners, rightCorners, status);
184+
if(this->getMinEigenVals())
185+
{
186+
updateStatus(leftCorners, rightCorners, status);
187+
}
188+
else
189+
{
190+
updateStatus(leftCorners, rightCorners, status, err);
191+
}
179192
return rightCorners;
180193
}
181194

@@ -227,14 +240,17 @@ std::vector<cv::Point2f> StereoOpticalFlow::computeCorrespondences(
227240
void StereoOpticalFlow::updateStatus(
228241
const std::vector<cv::Point2f> & leftCorners,
229242
const std::vector<cv::Point2f> & rightCorners,
230-
std::vector<unsigned char> & status) const
243+
std::vector<unsigned char> & status,
244+
std::vector<float> err) const
231245
{
232-
UASSERT(leftCorners.size() == rightCorners.size() && status.size() == leftCorners.size());
246+
UASSERT(
247+
leftCorners.size() == rightCorners.size() && status.size() == leftCorners.size() &&
248+
(err.empty() || err.size() == leftCorners.size()));
233249
int countFlowRejected = 0;
234250
int countDisparityRejected = 0;
235251
for(unsigned int i=0; i<status.size(); ++i)
236252
{
237-
if(status[i]!=0)
253+
if(status[i]!=0 && (err.empty() || err[i] < this->errorThreshold()))
238254
{
239255
float disparity = leftCorners[i].x - rightCorners[i].x;
240256
if(disparity <= this->minDisparity() || disparity > this->maxDisparity())

‎guilib/src/PreferencesDialog.cpp‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1678,6 +1678,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
16781678
_ui->stereo_maxDisparity->setObjectName(Parameters::kStereoMaxDisparity().c_str());
16791679
_ui->stereo_ssd->setObjectName(Parameters::kStereoSSD().c_str());
16801680
_ui->stereo_flow_eps->setObjectName(Parameters::kStereoEps().c_str());
1681+
_ui->stereo_flow_getMinEigenVals->setObjectName(Parameters::kStereoGetMinEigenVals().c_str());
1682+
_ui->stereo_flow_minEigThreshold->setObjectName(Parameters::kStereoMinEigThreshold().c_str());
1683+
_ui->stereo_flow_errorThreshold->setObjectName(Parameters::kStereoErrorThreshold().c_str());
16811684
_ui->stereo_opticalFlow->setObjectName(Parameters::kStereoOpticalFlow().c_str());
16821685
_ui->stereo_flow_gpu->setObjectName(Parameters::kStereoGpu().c_str());
16831686

‎guilib/src/ui/preferencesDialog.ui‎

Lines changed: 87 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -25532,10 +25532,17 @@ Lower the ratio -&gt; higher the precision.</string>
2553225532
</property>
2553325533
</widget>
2553425534
</item>
25535+
<item row="10" column="0">
25536+
<widget class="QCheckBox" name="stereo_flow_getMinEigenVals">
25537+
<property name="text">
25538+
<string/>
25539+
</property>
25540+
</widget>
25541+
</item>
2553525542
<item row="10" column="1">
25536-
<widget class="QLabel" name="label_stereo_flow_gpu">
25543+
<widget class="QLabel" name="label_788">
2553725544
<property name="text">
25538-
<string>[OpticalFlow = true] Do optical flow on GPU. RTAB-Map should be built with OpenCV CUDA.</string>
25545+
<string>[OpticalFlow = true] Use minimum eigen values as an error measure, otherwise L1 distance between patches is used as an error measure.</string>
2553925546
</property>
2554025547
<property name="wordWrap">
2554125548
<bool>true</bool>
@@ -25545,13 +25552,90 @@ Lower the ratio -&gt; higher the precision.</string>
2554525552
</property>
2554625553
</widget>
2554725554
</item>
25548-
<item row="10" column="0">
25555+
<item row="11" column="0">
25556+
<widget class="QDoubleSpinBox" name="stereo_flow_minEigThreshold">
25557+
<property name="decimals">
25558+
<number>6</number>
25559+
</property>
25560+
<property name="minimum">
25561+
<double>0.000000000000000</double>
25562+
</property>
25563+
<property name="maximum">
25564+
<double>1.000000000000000</double>
25565+
</property>
25566+
<property name="singleStep">
25567+
<double>0.000100000000000</double>
25568+
</property>
25569+
<property name="value">
25570+
<double>0.000100000000000</double>
25571+
</property>
25572+
</widget>
25573+
</item>
25574+
<item row="11" column="1">
25575+
<widget class="QLabel" name="label_789">
25576+
<property name="text">
25577+
<string>[GetMinEigenVals = true] If the minimum eigenvalue of a feature's spatial gradient matrix is less than minEigThreshold, then the feature is filtered out.</string>
25578+
</property>
25579+
<property name="wordWrap">
25580+
<bool>true</bool>
25581+
</property>
25582+
<property name="textInteractionFlags">
25583+
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
25584+
</property>
25585+
</widget>
25586+
</item>
25587+
<item row="12" column="0">
25588+
<widget class="QDoubleSpinBox" name="stereo_flow_errorThreshold">
25589+
<property name="decimals">
25590+
<number>6</number>
25591+
</property>
25592+
<property name="minimum">
25593+
<double>0.000000000000000</double>
25594+
</property>
25595+
<property name="maximum">
25596+
<double>9999.000000000000000</double>
25597+
</property>
25598+
<property name="singleStep">
25599+
<double>1.000000000000000</double>
25600+
</property>
25601+
<property name="value">
25602+
<double>50.000000000000000</double>
25603+
</property>
25604+
</widget>
25605+
</item>
25606+
<item row="12" column="1">
25607+
<widget class="QLabel" name="label_790">
25608+
<property name="text">
25609+
<string>[GetMinEigenVals = false] Filter out features with error greater than errorThreshold.</string>
25610+
</property>
25611+
<property name="wordWrap">
25612+
<bool>true</bool>
25613+
</property>
25614+
<property name="textInteractionFlags">
25615+
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
25616+
</property>
25617+
</widget>
25618+
</item>
25619+
<item row="13" column="0">
2554925620
<widget class="QCheckBox" name="stereo_flow_gpu">
2555025621
<property name="text">
2555125622
<string/>
2555225623
</property>
2555325624
</widget>
2555425625
</item>
25626+
<item row="13" column="1">
25627+
<widget class="QLabel" name="label_stereo_flow_gpu">
25628+
<property name="text">
25629+
<string>[OpticalFlow = true] Do optical flow on GPU. RTAB-Map should be built with OpenCV CUDA.</string>
25630+
</property>
25631+
<property name="wordWrap">
25632+
<bool>true</bool>
25633+
</property>
25634+
<property name="textInteractionFlags">
25635+
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
25636+
</property>
25637+
</widget>
25638+
</item>
2555525639
</layout>
2555625640
</widget>
2555725641
</item>

0 commit comments

Comments
 (0)