Stereo.cpp
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without
00006 modification, are permitted provided that the following conditions are met:
00007     * Redistributions of source code must retain the above copyright
00008       notice, this list of conditions and the following disclaimer.
00009     * Redistributions in binary form must reproduce the above copyright
00010       notice, this list of conditions and the following disclaimer in the
00011       documentation and/or other materials provided with the distribution.
00012     * Neither the name of the Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 */
00027 
00028 #include <rtabmap/core/Stereo.h>
00029 #include <rtabmap/core/util2d.h>
00030 #include <rtabmap/utilite/ULogger.h>
00031 #include <opencv2/video/tracking.hpp>
00032 
00033 namespace rtabmap {
00034 
00035 Stereo * Stereo::create(const ParametersMap & parameters)
00036 {
00037         bool opticalFlow = Parameters::defaultStereoOpticalFlow();
00038         Parameters::parse(parameters, Parameters::kStereoOpticalFlow(), opticalFlow);
00039         if(opticalFlow)
00040         {
00041                 return new StereoOpticalFlow(parameters);
00042         }
00043         else
00044         {
00045                 return new Stereo(parameters);
00046         }
00047 }
00048 
00049 Stereo::Stereo(const ParametersMap & parameters) :
00050                 winWidth_(Parameters::defaultStereoWinWidth()),
00051                 winHeight_(Parameters::defaultStereoWinHeight()),
00052                 iterations_(Parameters::defaultStereoIterations()),
00053                 maxLevel_(Parameters::defaultStereoMaxLevel()),
00054                 minDisparity_(Parameters::defaultStereoMinDisparity()),
00055                 maxDisparity_(Parameters::defaultStereoMaxDisparity()),
00056                 winSSD_(Parameters::defaultStereoSSD())
00057 {
00058         this->parseParameters(parameters);
00059 }
00060 
00061 void Stereo::parseParameters(const ParametersMap & parameters)
00062 {
00063         Parameters::parse(parameters, Parameters::kStereoWinWidth(), winWidth_);
00064         Parameters::parse(parameters, Parameters::kStereoWinHeight(), winHeight_);
00065         Parameters::parse(parameters, Parameters::kStereoIterations(), iterations_);
00066         Parameters::parse(parameters, Parameters::kStereoMaxLevel(), maxLevel_);
00067         Parameters::parse(parameters, Parameters::kStereoMinDisparity(), minDisparity_);
00068         Parameters::parse(parameters, Parameters::kStereoMaxDisparity(), maxDisparity_);
00069         Parameters::parse(parameters, Parameters::kStereoSSD(), winSSD_);
00070 }
00071 
00072 std::vector<cv::Point2f> Stereo::computeCorrespondences(
00073                 const cv::Mat & leftImage,
00074                 const cv::Mat & rightImage,
00075                 const std::vector<cv::Point2f> & leftCorners,
00076                 std::vector<unsigned char> & status) const
00077 {
00078         std::vector<cv::Point2f> rightCorners;
00079         UDEBUG("util2d::calcStereoCorrespondences() begin");
00080         rightCorners = util2d::calcStereoCorrespondences(
00081                                         leftImage,
00082                                         rightImage,
00083                                         leftCorners,
00084                                         status,
00085                                         cv::Size(winWidth_, winHeight_),
00086                                         maxLevel_,
00087                                         iterations_,
00088                                         minDisparity_,
00089                                         maxDisparity_,
00090                                         winSSD_);
00091         UDEBUG("util2d::calcStereoCorrespondences() end");
00092         return rightCorners;
00093 }
00094 
00095 StereoOpticalFlow::StereoOpticalFlow(const ParametersMap & parameters) :
00096                 Stereo(parameters),
00097                 epsilon_(Parameters::defaultStereoEps())
00098 {
00099         this->parseParameters(parameters);
00100 }
00101 
00102 void StereoOpticalFlow::parseParameters(const ParametersMap & parameters)
00103 {
00104         Stereo::parseParameters(parameters);
00105         Parameters::parse(parameters, Parameters::kStereoEps(), epsilon_);
00106 }
00107 
00108 
00109 std::vector<cv::Point2f> StereoOpticalFlow::computeCorrespondences(
00110                 const cv::Mat & leftImage,
00111                 const cv::Mat & rightImage,
00112                 const std::vector<cv::Point2f> & leftCorners,
00113                 std::vector<unsigned char> & status) const
00114 {
00115         std::vector<cv::Point2f> rightCorners;
00116         UDEBUG("util2d::calcOpticalFlowPyrLKStereo() begin");
00117         std::vector<float> err;
00118         util2d::calcOpticalFlowPyrLKStereo(
00119                         leftImage,
00120                         rightImage,
00121                         leftCorners,
00122                         rightCorners,
00123                         status,
00124                         err,
00125                         this->winSize(),
00126                         this->maxLevel(),
00127                         cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, this->iterations(), epsilon_),
00128                         cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
00129         UDEBUG("util2d::calcOpticalFlowPyrLKStereo() end");
00130         UASSERT(leftCorners.size() == rightCorners.size() && status.size() == leftCorners.size());
00131         int countFlowRejected = 0;
00132         int countDisparityRejected = 0;
00133         for(unsigned int i=0; i<status.size(); ++i)
00134         {
00135                 if(status[i]!=0)
00136                 {
00137                         float disparity = leftCorners[i].x - rightCorners[i].x;
00138                         if(disparity <= this->minDisparity() || disparity > this->maxDisparity())
00139                         {
00140                                 status[i] = 0;
00141                                 ++countDisparityRejected;
00142                         }
00143                 }
00144                 else
00145                 {
00146                         ++countFlowRejected;
00147                 }
00148         }
00149         UDEBUG("total=%d countFlowRejected=%d countDisparityRejected=%d", (int)status.size(), countFlowRejected, countDisparityRejected);
00150 
00151         if(countFlowRejected + countDisparityRejected > (int)status.size()/2)
00152         {
00153                 UWARN("A large number (%d/%d) of stereo correspondences are rejected! "
00154                                 "Optical flow may have failed because images are not calibrated, "
00155                                 "the background is too far (no disparity between the images), "
00156                                 "maximum disparity may be too small (%f) or that exposure between "
00157                                 "left and right images is too different.",
00158                                 countFlowRejected+countDisparityRejected,
00159                                 (int)status.size(),
00160                                 this->maxDisparity());
00161         }
00162 
00163         return rightCorners;
00164 }
00165 
00166 } /* namespace rtabmap */


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jun 6 2019 21:59:31