Stereo.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include <rtabmap/core/Stereo.h>
29 #include <rtabmap/core/util2d.h>
31 #include <opencv2/video/tracking.hpp>
32 
33 namespace rtabmap {
34 
35 Stereo * Stereo::create(const ParametersMap & parameters)
36 {
37  bool opticalFlow = Parameters::defaultStereoOpticalFlow();
38  Parameters::parse(parameters, Parameters::kStereoOpticalFlow(), opticalFlow);
39  if(opticalFlow)
40  {
41  return new StereoOpticalFlow(parameters);
42  }
43  else
44  {
45  return new Stereo(parameters);
46  }
47 }
48 
49 Stereo::Stereo(const ParametersMap & parameters) :
50  winWidth_(Parameters::defaultStereoWinWidth()),
51  winHeight_(Parameters::defaultStereoWinHeight()),
52  iterations_(Parameters::defaultStereoIterations()),
53  maxLevel_(Parameters::defaultStereoMaxLevel()),
54  minDisparity_(Parameters::defaultStereoMinDisparity()),
55  maxDisparity_(Parameters::defaultStereoMaxDisparity()),
56  winSSD_(Parameters::defaultStereoSSD())
57 {
58  this->parseParameters(parameters);
59 }
60 
61 void Stereo::parseParameters(const ParametersMap & parameters)
62 {
63  Parameters::parse(parameters, Parameters::kStereoWinWidth(), winWidth_);
64  Parameters::parse(parameters, Parameters::kStereoWinHeight(), winHeight_);
65  Parameters::parse(parameters, Parameters::kStereoIterations(), iterations_);
66  Parameters::parse(parameters, Parameters::kStereoMaxLevel(), maxLevel_);
67  Parameters::parse(parameters, Parameters::kStereoMinDisparity(), minDisparity_);
68  Parameters::parse(parameters, Parameters::kStereoMaxDisparity(), maxDisparity_);
69  Parameters::parse(parameters, Parameters::kStereoSSD(), winSSD_);
70 }
71 
72 std::vector<cv::Point2f> Stereo::computeCorrespondences(
73  const cv::Mat & leftImage,
74  const cv::Mat & rightImage,
75  const std::vector<cv::Point2f> & leftCorners,
76  std::vector<unsigned char> & status) const
77 {
78  std::vector<cv::Point2f> rightCorners;
79  UDEBUG("util2d::calcStereoCorrespondences() begin");
80  rightCorners = util2d::calcStereoCorrespondences(
81  leftImage,
82  rightImage,
83  leftCorners,
84  status,
85  cv::Size(winWidth_, winHeight_),
86  maxLevel_,
90  winSSD_);
91  UDEBUG("util2d::calcStereoCorrespondences() end");
92  return rightCorners;
93 }
94 
96  Stereo(parameters),
97  epsilon_(Parameters::defaultStereoEps())
98 {
99  this->parseParameters(parameters);
100 }
101 
103 {
104  Stereo::parseParameters(parameters);
105  Parameters::parse(parameters, Parameters::kStereoEps(), epsilon_);
106 }
107 
108 
110  const cv::Mat & leftImage,
111  const cv::Mat & rightImage,
112  const std::vector<cv::Point2f> & leftCorners,
113  std::vector<unsigned char> & status) const
114 {
115  std::vector<cv::Point2f> rightCorners;
116  UDEBUG("util2d::calcOpticalFlowPyrLKStereo() begin");
117  std::vector<float> err;
119  leftImage,
120  rightImage,
121  leftCorners,
122  rightCorners,
123  status,
124  err,
125  this->winSize(),
126  this->maxLevel(),
127  cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, this->iterations(), epsilon_),
128  cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
129  UDEBUG("util2d::calcOpticalFlowPyrLKStereo() end");
130  UASSERT(leftCorners.size() == rightCorners.size() && status.size() == leftCorners.size());
131  int countFlowRejected = 0;
132  int countDisparityRejected = 0;
133  for(unsigned int i=0; i<status.size(); ++i)
134  {
135  if(status[i]!=0)
136  {
137  float disparity = leftCorners[i].x - rightCorners[i].x;
138  if(disparity <= this->minDisparity() || disparity > this->maxDisparity())
139  {
140  status[i] = 0;
141  ++countDisparityRejected;
142  }
143  }
144  else
145  {
146  ++countFlowRejected;
147  }
148  }
149  UDEBUG("total=%d countFlowRejected=%d countDisparityRejected=%d", (int)status.size(), countFlowRejected, countDisparityRejected);
150 
151  return rightCorners;
152 }
153 
154 } /* namespace rtabmap */
rtabmap::Stereo::Stereo
Stereo(const ParametersMap &parameters=ParametersMap())
Definition: Stereo.cpp:49
rtabmap::Stereo::minDisparity_
float minDisparity_
Definition: Stereo.h:65
rtabmap::Stereo
Definition: Stereo.h:38
rtabmap::Stereo::maxLevel
int maxLevel() const
Definition: Stereo.h:55
rtabmap::Stereo::maxDisparity_
float maxDisparity_
Definition: Stereo.h:66
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:500
rtabmap::Stereo::winHeight_
int winHeight_
Definition: Stereo.h:62
rtabmap::Stereo::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: Stereo.cpp:61
rtabmap::Stereo::winSize
cv::Size winSize() const
Definition: Stereo.h:53
rtabmap::Stereo::maxLevel_
int maxLevel_
Definition: Stereo.h:64
rtabmap::StereoOpticalFlow::StereoOpticalFlow
StereoOpticalFlow(const ParametersMap &parameters=ParametersMap())
Definition: Stereo.cpp:95
rtabmap::StereoOpticalFlow::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: Stereo.cpp:102
rtabmap::util2d::calcStereoCorrespondences
std::vector< cv::Point2f > RTABMAP_CORE_EXPORT calcStereoCorrespondences(const cv::Mat &leftImage, const cv::Mat &rightImage, const std::vector< cv::Point2f > &leftCorners, std::vector< unsigned char > &status, cv::Size winSize=cv::Size(6, 3), int maxLevel=3, int iterations=5, float minDisparity=0.0f, float maxDisparity=64.0f, bool ssdApproach=true)
Definition: util2d.cpp:122
rtabmap::StereoOpticalFlow::epsilon_
float epsilon_
Definition: Stereo.h:85
UASSERT
#define UASSERT(condition)
Stereo.h
rtabmap::Parameters
Definition: Parameters.h:170
rtabmap::StereoOpticalFlow
Definition: Stereo.h:70
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
ULogger.h
ULogger class and convenient macros.
util2d.h
rtabmap::util2d::calcOpticalFlowPyrLKStereo
void RTABMAP_CORE_EXPORT calcOpticalFlowPyrLKStereo(cv::InputArray _prevImg, cv::InputArray _nextImg, cv::InputArray _prevPts, cv::InputOutputArray _nextPts, cv::OutputArray _status, cv::OutputArray _err, cv::Size winSize=cv::Size(15, 3), int maxLevel=3, cv::TermCriteria criteria=cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 30, 0.01), int flags=0, double minEigThreshold=1e-4)
Definition: util2d.cpp:371
rtabmap::Stereo::winWidth_
int winWidth_
Definition: Stereo.h:61
rtabmap::Stereo::winSSD_
bool winSSD_
Definition: Stereo.h:67
rtabmap::Stereo::iterations_
int iterations_
Definition: Stereo.h:63
UDEBUG
#define UDEBUG(...)
rtabmap::Stereo::create
static Stereo * create(const ParametersMap &parameters=ParametersMap())
Definition: Stereo.cpp:35
rtabmap::Stereo::maxDisparity
float maxDisparity() const
Definition: Stereo.h:57
rtabmap
Definition: CameraARCore.cpp:35
rtabmap::Stereo::iterations
int iterations() const
Definition: Stereo.h:54
rtabmap::Stereo::computeCorrespondences
virtual std::vector< cv::Point2f > computeCorrespondences(const cv::Mat &leftImage, const cv::Mat &rightImage, const std::vector< cv::Point2f > &leftCorners, std::vector< unsigned char > &status) const
Definition: Stereo.cpp:72
rtabmap::StereoOpticalFlow::computeCorrespondences
virtual std::vector< cv::Point2f > computeCorrespondences(const cv::Mat &leftImage, const cv::Mat &rightImage, const std::vector< cv::Point2f > &leftCorners, std::vector< unsigned char > &status) const
Definition: Stereo.cpp:109
rtabmap::Stereo::minDisparity
float minDisparity() const
Definition: Stereo.h:56
i
int i


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:22