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 */
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:497
float maxDisparity() const
Definition: Stereo.h:57
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
int maxLevel() const
Definition: Stereo.h:55
int iterations() const
Definition: Stereo.h:54
Stereo(const ParametersMap &parameters=ParametersMap())
Definition: Stereo.cpp:49
float minDisparity_
Definition: Stereo.h:65
GLM_FUNC_DECL genType e()
float maxDisparity_
Definition: Stereo.h:66
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
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
int winHeight_
Definition: Stereo.h:62
int maxLevel_
Definition: Stereo.h:64
#define UASSERT(condition)
virtual void parseParameters(const ParametersMap &parameters)
Definition: Stereo.cpp:61
cv::Size winSize() const
Definition: Stereo.h:53
static Stereo * create(const ParametersMap &parameters=ParametersMap())
Definition: Stereo.cpp:35
bool winSSD_
Definition: Stereo.h:67
int winWidth_
Definition: Stereo.h:61
#define UDEBUG(...)
ULogger class and convenient macros.
StereoOpticalFlow(const ParametersMap &parameters=ParametersMap())
Definition: Stereo.cpp:95
int iterations_
Definition: Stereo.h:63
std::vector< cv::Point2f > RTABMAP_EXP 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
virtual void parseParameters(const ParametersMap &parameters)
Definition: Stereo.cpp:102
float minDisparity() const
Definition: Stereo.h:56
void RTABMAP_EXP 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
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:38:57