i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add params to filter out poor stereo flow matches (#1667)

Browse Source 
* Add params to filter out poor stereo flow matches

* Added parameters: Vis/CorFlowUseMinEigenVals, Vis/CorFlowMinEigThreshold, Vis/CorFlowErrorThreshold. Renamed Stereo/GetMinEigenVals to Stereo/UseMinEigenVals

* Setting suggested default value of 20 for Vis/CorFlowErrorThreshold

---------

Co-authored-by: matlabbe <matlabbe@gmail.com>
This commit is contained in:
Borong Yuan
2026-03-18 12:53:14 +08:00
committed by GitHub
parent 0151f8cdfb
commit 5b985f69be
7 changed files with 318 additions and 89 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
corelib/src/RegistrationVis.cpp
View File

@@ -84,6 +84,9 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
_flowEps(Parameters::defaultVisCorFlowEps()),
_flowMaxLevel(Parameters::defaultVisCorFlowMaxLevel()),
_flowGpu(Parameters::defaultVisCorFlowGpu()),
_flowUseMinEigenVals(Parameters::defaultVisCorFlowUseMinEigenVals()),
_flowMinEigThreshold(Parameters::defaultVisCorFlowMinEigThreshold()),
_flowErrorThreshold(Parameters::defaultVisCorFlowErrorThreshold()),
_nndr(Parameters::defaultVisCorNNDR()),
_nnType(Parameters::defaultVisCorNNType()),
_gmsWithRotation(Parameters::defaultGMSWithRotation()),
@@ -145,6 +148,9 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kVisCorFlowEps(), _flowEps);
Parameters::parse(parameters, Parameters::kVisCorFlowMaxLevel(), _flowMaxLevel);
Parameters::parse(parameters, Parameters::kVisCorFlowGpu(), _flowGpu);
Parameters::parse(parameters, Parameters::kVisCorFlowUseMinEigenVals(), _flowUseMinEigenVals);
Parameters::parse(parameters, Parameters::kVisCorFlowMinEigThreshold(), _flowMinEigThreshold);
Parameters::parse(parameters, Parameters::kVisCorFlowErrorThreshold(), _flowErrorThreshold);
Parameters::parse(parameters, Parameters::kVisCorNNDR(), _nndr);
Parameters::parse(parameters, Parameters::kVisCorNNType(), _nnType);
Parameters::parse(parameters, Parameters::kGMSWithRotation(), _gmsWithRotation);
@@ -656,6 +662,7 @@ Transform RegistrationVis::computeTransformationImpl(
// Find features in the new left image
UDEBUG("guessSet = %d", guessSet?1:0);
std::vector<unsigned char> status;
std::vector<float> err;
#ifdef HAVE_OPENCV_CUDAOPTFLOW
if (_flowGpu)
{
@@ -685,7 +692,6 @@ Transform RegistrationVis::computeTransformationImpl(
else
#endif
{
std::vector<float> err;
UDEBUG("cv::calcOpticalFlowPyrLK() begin");
cv::calcOpticalFlowPyrLK(
imageFrom,
@@ -697,7 +703,8 @@ Transform RegistrationVis::computeTransformationImpl(
cv::Size(_flowWinSize, _flowWinSize),
guessSet ? 0 : _flowMaxLevel,
cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, _flowIterations, _flowEps),
cv::OPTFLOW_LK_GET_MIN_EIGENVALS | (guessSet ? cv::OPTFLOW_USE_INITIAL_FLOW : 0), 1e-4);
(_flowUseMinEigenVals ? cv::OPTFLOW_LK_GET_MIN_EIGENVALS : 0) | (guessSet ? cv::OPTFLOW_USE_INITIAL_FLOW : 0),
_flowMinEigThreshold);
UDEBUG("cv::calcOpticalFlowPyrLK() end");
}
@@ -706,11 +713,14 @@ Transform RegistrationVis::computeTransformationImpl(
std::vector<cv::Point3f> kptsFrom3DKept(kptsFrom3D.size());
std::vector<int> orignalWordsFromIdsCpy = orignalWordsFromIds;
int ki = 0;
UASSERT((status.empty() || cornersTo.size() == status.size()) &&
(err.empty() || cornersTo.size() == err.size()));
for(unsigned int i=0; i<status.size(); ++i)
{
if(status[i] &&
uIsInBounds(cornersTo[i].x, 0.0f, float(imageTo.cols)) &&
uIsInBounds(cornersTo[i].y, 0.0f, float(imageTo.rows)))
uIsInBounds(cornersTo[i].y, 0.0f, float(imageTo.rows)) &&
(_flowUseMinEigenVals || err.empty() || err[i] < _flowErrorThreshold))
{
if(orignalWordsFromIdsCpy.size())
{

28
corelib/src/Stereo.cpp
View File

@@ -113,6 +113,9 @@ std::vector<cv::Point2f> Stereo::computeCorrespondences(
StereoOpticalFlow::StereoOpticalFlow(const ParametersMap & parameters) :
Stereo(parameters),
epsilon_(Parameters::defaultStereoEps()),
useMinEigenVals_(Parameters::defaultStereoUseMinEigenVals()),
minEigThreshold_(Parameters::defaultStereoMinEigThreshold()),
errorThreshold_(Parameters::defaultStereoErrorThreshold()),
gpu_(Parameters::defaultStereoGpu())
{
this->parseParameters(parameters);
@@ -122,6 +125,9 @@ void StereoOpticalFlow::parseParameters(const ParametersMap & parameters)
{
Stereo::parseParameters(parameters);
Parameters::parse(parameters, Parameters::kStereoEps(), epsilon_);
Parameters::parse(parameters, Parameters::kStereoUseMinEigenVals(), useMinEigenVals_);
Parameters::parse(parameters, Parameters::kStereoMinEigThreshold(), minEigThreshold_);
Parameters::parse(parameters, Parameters::kStereoErrorThreshold(), errorThreshold_);
Parameters::parse(parameters, Parameters::kStereoGpu(), gpu_);
#ifndef HAVE_OPENCV_CUDAOPTFLOW
if(gpu_)
@@ -171,11 +177,18 @@ std::vector<cv::Point2f> StereoOpticalFlow::computeCorrespondences(
err,
this->winSize(),
this->maxLevel(),
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, this->iterations(), epsilon_),
cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, this->iterations(), this->epsilon()),
this->usingMinEigenVals() ? cv::OPTFLOW_LK_GET_MIN_EIGENVALS : 0, this->minEigThreshold());
UDEBUG("util2d::calcOpticalFlowPyrLKStereo() end");
}
updateStatus(leftCorners, rightCorners, status);
if(this->usingMinEigenVals())
{
updateStatus(leftCorners, rightCorners, status);
}
else
{
updateStatus(leftCorners, rightCorners, status, err);
}
return rightCorners;
}
@@ -227,14 +240,17 @@ std::vector<cv::Point2f> StereoOpticalFlow::computeCorrespondences(
void StereoOpticalFlow::updateStatus(
const std::vector<cv::Point2f> & leftCorners,
const std::vector<cv::Point2f> & rightCorners,
std::vector<unsigned char> & status) const
std::vector<unsigned char> & status,
std::vector<float> err) const
{
UASSERT(leftCorners.size() == rightCorners.size() && status.size() == leftCorners.size());
UASSERT(
leftCorners.size() == rightCorners.size() && status.size() == leftCorners.size() &&
(err.empty() || err.size() == leftCorners.size()));
int countFlowRejected = 0;
int countDisparityRejected = 0;
for(unsigned int i=0; i<status.size(); ++i)
{
if(status[i]!=0)
if(status[i]!=0 && (err.empty() || err[i] < this->errorThreshold()))
{
float disparity = leftCorners[i].x - rightCorners[i].x;
if(disparity <= this->minDisparity() || disparity > this->maxDisparity())