Added Stereo and StereoDense base classes (with Stereo->StereoOpticalFlow and StereoDense->StereoBM) to handle easily stereo parameters. Added stereoEval tool to test Stereo/OpticalFlow=false or true. Added StereoBM parameters. Refactoring of the Preferences dialog (tree view order and some titles)

2026-10-05 17:27:46 +08:00 · 2015-12-11 17:44:15 -05:00
parent 818eda005f
commit 9cc94ce1e6
44 changed files with 6866 additions and 4311 deletions
@@ -0,0 +1,134 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/Stereo.h>
+#include <rtabmap/core/util2d.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <opencv2/video/tracking.hpp>
+
+namespace rtabmap {
+
+Stereo::Stereo(const ParametersMap & parameters) :
+		winWidth_(Parameters::defaultStereoWinWidth()),
+		winHeight_(Parameters::defaultStereoWinHeight()),
+		iterations_(Parameters::defaultStereoIterations()),
+		maxLevel_(Parameters::defaultStereoMaxLevel()),
+		minDisparity_(Parameters::defaultStereoMinDisparity()),
+		maxDisparity_(Parameters::defaultStereoMaxDisparity()),
+		winSSD_(Parameters::defaultStereoSSD())
+{
+}
+
+void Stereo::parseParameters(const ParametersMap & parameters)
+{
+	Parameters::parse(parameters, Parameters::kStereoWinWidth(), winWidth_);
+	Parameters::parse(parameters, Parameters::kStereoWinHeight(), winHeight_);
+	Parameters::parse(parameters, Parameters::kStereoIterations(), iterations_);
+	Parameters::parse(parameters, Parameters::kStereoMaxLevel(), maxLevel_);
+	Parameters::parse(parameters, Parameters::kStereoMinDisparity(), minDisparity_);
+	Parameters::parse(parameters, Parameters::kStereoMaxDisparity(), maxDisparity_);
+	Parameters::parse(parameters, Parameters::kStereoSSD(), winSSD_);
+}
+
+std::vector<cv::Point2f> Stereo::computeCorrespondences(
+		const cv::Mat & leftImage,
+		const cv::Mat & rightImage,
+		const std::vector<cv::Point2f> & leftCorners,
+		std::vector<unsigned char> & status) const
+{
+	std::vector<cv::Point2f> rightCorners;
+	UDEBUG("util2d::calcStereoCorrespondences() begin");
+	rightCorners = util2d::calcStereoCorrespondences(
+					leftImage,
+					rightImage,
+					leftCorners,
+					status,
+					cv::Size(winWidth_, winHeight_),
+					maxLevel_,
+					iterations_,
+					minDisparity_,
+					maxDisparity_,
+					winSSD_);
+	UDEBUG("util2d::calcStereoCorrespondences() end");
+	return rightCorners;
+}
+
+StereoOpticalFlow::StereoOpticalFlow(const ParametersMap & parameters) :
+		Stereo(parameters),
+		epsilon_(Parameters::defaultStereoEps()),
+		maxSlope_(Parameters::defaultStereoMaxSlope())
+{
+	this->parseParameters(parameters);
+}
+
+void StereoOpticalFlow::parseParameters(const ParametersMap & parameters)
+{
+	Stereo::parseParameters(parameters);
+	Parameters::parse(parameters, Parameters::kStereoEps(), epsilon_);
+	Parameters::parse(parameters, Parameters::kStereoMaxSlope(), maxSlope_);
+}
+
+
+std::vector<cv::Point2f> StereoOpticalFlow::computeCorrespondences(
+		const cv::Mat & leftImage,
+		const cv::Mat & rightImage,
+		const std::vector<cv::Point2f> & leftCorners,
+		std::vector<unsigned char> & status) const
+{
+	std::vector<cv::Point2f> rightCorners;
+	UDEBUG("util2d::calcOpticalFlowPyrLKStereo() begin");
+	std::vector<float> err;
+	util2d::calcOpticalFlowPyrLKStereo(
+			leftImage,
+			rightImage,
+			leftCorners,
+			rightCorners,
+			status,
+			err,
+			this->winSize(),
+			this->maxLevel(),
+			cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, this->iterations(), epsilon_),
+			cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
+	UDEBUG("util2d::calcOpticalFlowPyrLKStereo() end");
+	UASSERT(leftCorners.size() == rightCorners.size() && status.size() == leftCorners.size());
+	for(unsigned int i=0; i<status.size(); ++i)
+	{
+		if(status[i]!=0)
+		{
+			float disparity = leftCorners[i].x - rightCorners[i].x;
+			float slope = fabs((leftCorners[i].y-rightCorners[i].y) / (leftCorners[i].x-rightCorners[i].x));
+			if(disparity < float(this->minDisparity()) || disparity > float(this->maxDisparity()) ||
+			   (maxSlope_ > 0.0f && fabs(leftCorners[i].y-rightCorners[i].y) > 1.0f && slope > maxSlope_))
+			{
+				status[i] = 0;
+			}
+		}
+	}
+	return rightCorners;
+}
+
+} /* namespace rtabmap */