tools/StereoEval/main.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 "io.h"
29 #include <rtabmap/core/Parameters.h>
30 #include <rtabmap/core/Features2d.h>
31 #include <rtabmap/core/util2d.h>
32 #include <rtabmap/core/CameraModel.h>
33 #include <rtabmap/core/Stereo.h>
34 #include <rtabmap/core/StereoCameraModel.h>
35 #include <rtabmap/utilite/ULogger.h>
36 #include <rtabmap/utilite/UStl.h>
37 #include <rtabmap/utilite/UConversion.h>
38 #include <rtabmap/utilite/UTimer.h>
39 #include <rtabmap/utilite/UMath.h>
40 #include <opencv2/imgproc/types_c.h>
41 #include <fstream>
42 #include <string>
43 
44 using namespace rtabmap;
45 
46 void showUsage()
47 {
48  printf("Usage:\n"
49  "evalStereo.exe left.png right.png calib.txt disp.pfm mask.png [Parameters]\n"
50  "Example (with http://vision.middlebury.edu/stereo datasets):\n"
51  " $ ./rtabmap-stereoEval im0.png im1.png calib.txt disp0GT.pfm mask0nocc.png -Kp/DetectorStrategy 6 -Stereo/WinSize 5 -Stereo/MaxLevel 2 -Kp/WordsPerImage 1000 -Stereo/OpticalFlow false -Stereo/Iterations 5\n\n");
52  exit(1);
53 }
54 
55 int main(int argc, char * argv[])
56 {
57  ULogger::setLevel(ULogger::kDebug);
58  ULogger::setType(ULogger::kTypeConsole);
59 
60  if(argc < 6)
61  {
62  showUsage();
63  }
64 
65  ParametersMap parameters = Parameters::getDefaultParameters();
66  for(int i=6; i<argc; ++i)
67  {
68  // Check for RTAB-Map's parameters
69  std::string key = argv[i];
70  key = uSplit(key, '-').back();
71  if(parameters.find(key) != parameters.end())
72  {
73  ++i;
74  if(i < argc)
75  {
76  std::string value = argv[i];
77  if(value.empty())
78  {
79  showUsage();
80  }
81  else
82  {
83  value = uReplaceChar(value, ',', ' ');
84  }
85  std::pair<ParametersMap::iterator, bool> inserted = parameters.insert(ParametersPair(key, value));
86  if(inserted.second == false)
87  {
88  inserted.first->second = value;
89  }
90  }
91  else
92  {
93  showUsage();
94  }
95  continue;
96  }
97 
98  //backward compatibility
99  // look for old parameter name
100  std::map<std::string, std::pair<bool, std::string> >::const_iterator oldIter = Parameters::getRemovedParameters().find(key);
101  if(oldIter!=Parameters::getRemovedParameters().end())
102  {
103  ++i;
104  if(i < argc)
105  {
106  std::string value = argv[i];
107  if(value.empty())
108  {
109  showUsage();
110  }
111  else
112  {
113  value = uReplaceChar(value, ',', ' ');
114  }
115 
116  if(oldIter->second.first)
117  {
118  key = oldIter->second.second;
119  UWARN("Parameter migration from \"%s\" to \"%s\" (value=%s).",
120  oldIter->first.c_str(), oldIter->second.second.c_str(), value.c_str());
121  }
122  else if(oldIter->second.second.empty())
123  {
124  UERROR("Parameter \"%s\" doesn't exist anymore.", oldIter->first.c_str());
125  }
126  else
127  {
128  UERROR("Parameter \"%s\" doesn't exist anymore, check this similar parameter \"%s\".", oldIter->first.c_str(), oldIter->second.second.c_str());
129  }
130  if(oldIter->second.first)
131  {
132  std::pair<ParametersMap::iterator, bool> inserted = parameters.insert(ParametersPair(key, value));
133  if(inserted.second == false)
134  {
135  inserted.first->second = value;
136  }
137  }
138  }
139  else
140  {
141  showUsage();
142  }
143  continue;
144  }
145 
146  printf("Unrecognized option : %s\n", argv[i]);
147  showUsage();
148  }
149 
150  UINFO("Loading files...");
151 
152  cv::Mat left = cv::imread(argv[1]);
153  cv::Mat right = cv::imread(argv[2]);
154  cv::Mat disp = readPFM(argv[4]);
155  cv::Mat mask = cv::imread(argv[5]);
156 
157  if(!left.empty() && !right.empty() && !disp.empty() && !mask.empty())
158  {
159  UASSERT(left.rows == disp.rows);
160  UASSERT(left.cols == disp.cols);
161  UASSERT(disp.rows == mask.rows);
162  UASSERT(disp.cols == mask.cols);
163 
164  // read calib.txt
165  // Example format:
166  // --- calib.txt:
167  // cam0=[1038.018 0 322.037; 0 1038.018 243.393; 0 0 1]
168  // cam1=[1038.018 0 375.308; 0 1038.018 243.393; 0 0 1]
169  // doffs=53.271
170  // baseline=176.252
171  // width=718
172  // height=496
173  // ndisp=73
174  // isint=0
175  // vmin=8
176  // vmax=65
177  // dyavg=0.184
178  // dymax=0.423
179  // ---
180  std::string calibFile = argv[3];
181  std::ifstream stream(calibFile.c_str());
182  std::string line;
183 
184  // two first lines are camera intrinsics
185  UINFO("Loading calibration... (%s)", calibFile.c_str());
186  std::vector<cv::Mat> K(2);
187  for(int i=0; i<2; ++i)
188  {
189  getline(stream, line);
190  line.erase(0, 6);
191  line = uReplaceChar(line, ']', "");
192  line = uReplaceChar(line, ';', "");
193  UINFO("K[%d] = %s", i, line.c_str());
194  std::vector<std::string> valuesStr = uListToVector(uSplit(line, ' '));
195  UASSERT(valuesStr.size() == 9);
196  K[i] = cv::Mat(3,3,CV_64FC1);
197  for(unsigned int j=0; j<valuesStr.size(); ++j)
198  {
199  K[i].at<double>(j) = uStr2Double(valuesStr[j]);
200  }
201  }
202 
203  // skip doffs line
204  getline(stream, line);
205 
206  // baseline
207  getline(stream, line);
208  line.erase(0, 9);
209  double baseline = uStr2Double(line);
210  UINFO("Baseline = %f", baseline);
211 
212  StereoCameraModel model(
213  calibFile,
214  CameraModel(K[0].at<double>(0,0), K[0].at<double>(1,1), K[0].at<double>(0,2), K[0].at<double>(1,2)),
215  CameraModel(K[1].at<double>(0,0), K[1].at<double>(1,1), K[1].at<double>(0,2), K[1].at<double>(1,2), Transform::getIdentity(), -baseline/K[1].at<double>(0,0)));
216 
217  UASSERT(model.isValidForProjection());
218 
219  UINFO("Processing...");
220 
221  // Processing...
222  cv::Mat leftMono;
223  if(left.channels() == 3)
224  {
225  cv::cvtColor(left, leftMono, CV_BGR2GRAY);
226  }
227  else
228  {
229  leftMono = left;
230  }
231  cv::Mat rightMono;
232  if(right.channels() == 3)
233  {
234  cv::cvtColor(right, rightMono, CV_BGR2GRAY);
235  }
236  else
237  {
238  rightMono = right;
239  }
240 
241  UTimer timer;
242  double timeKpts;
243  double timeSubPixel;
244  double timeStereo;
245 
246  // generate kpts
247  std::vector<cv::KeyPoint> kpts;
248  uInsert(parameters, ParametersPair(Parameters::kKpRoiRatios(), "0.03 0.03 0.04 0.04"));
249  Feature2D * kptDetector = Feature2D::create(parameters);
250  kpts = kptDetector->generateKeypoints(leftMono);
251  delete kptDetector;
252 
253  timeKpts = timer.ticks();
254 
255  std::vector<cv::Point2f> leftCorners(kpts.size());
256  cv::KeyPoint::convert(kpts, leftCorners);
257  int subPixWinSize = 0;
258  int subPixIterations = 0;
259  double subPixEps = 0;
260  Parameters::parse(parameters, Parameters::kKpSubPixWinSize(), subPixWinSize);
261  Parameters::parse(parameters, Parameters::kKpSubPixIterations(), subPixIterations);
262  Parameters::parse(parameters, Parameters::kKpSubPixEps(), subPixEps);
263  if(subPixWinSize > 0 && subPixIterations > 0)
264  {
265  UDEBUG("cv::cornerSubPix() begin");
266  cv::cornerSubPix(leftMono, leftCorners,
267  cv::Size( subPixWinSize, subPixWinSize ),
268  cv::Size( -1, -1 ),
269  cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, subPixIterations, subPixEps ) );
270  UDEBUG("cv::cornerSubPix() end");
271  }
272 
273  timeSubPixel = timer.ticks();
274 
275  // Find features in the new right image
276  std::vector<unsigned char> status;
277  std::vector<cv::Point2f> rightCorners;
278 
279  bool opticalFlow = false;
280  Parameters::parse(parameters, Parameters::kStereoOpticalFlow(), opticalFlow);
281  Stereo * stereo = 0;
282  if(opticalFlow)
283  {
284  stereo = new StereoOpticalFlow(parameters);
285  }
286  else
287  {
288  stereo = new Stereo(parameters);
289  }
290 
291  rightCorners = stereo->computeCorrespondences(
292  leftMono,
293  rightMono,
294  leftCorners,
295  status);
296  delete stereo;
297 
298  timeStereo = timer.ticks();
299 
300  UINFO("Time: kpts:%f s, subpix=%f s, stereo=%f s", timeKpts, timeSubPixel, timeStereo);
301 
302  UDEBUG("Mask = %d", mask.type());
303 
304  int inliers = 0;
305  int subInliers = 0;
306  int badInliers = 0;
307  float sumInliers = 0.0f;
308  float sumSubInliers = 0.0f;
309  int goodRejected = 0;
310  int badRejected = 0;
311  for(unsigned int i=0; i<leftCorners.size(); ++i)
312  {
313  float gt = disp.at<float>(int(rightCorners[i].y), int(leftCorners[i].x));
314  if(status[i]!=0)
315  {
316  float d = leftCorners[i].x - rightCorners[i].x;
317  //float err = fabs(d-gt);
318  //UDEBUG("Pt(%f,%f): d=%f, gt=%f, error=%f", leftCorners[i].x, leftCorners[i].y, d, gt, err);
319 
320  if(uIsFinite(gt))
321  {
322  if(fabs(d-gt) < 1.0f)
323  {
324  cv::line(left,
325  leftCorners[i],
326  cv::Point2f(leftCorners[i].x - gt, leftCorners[i].y),
327  cv::Scalar( 0, 255, 0 ));
328 
329  cv::line(left,
330  cv::Point2f(leftCorners[i].x - (d<gt?d:gt), leftCorners[i].y),
331  cv::Point2f(leftCorners[i].x - (d>gt?d:gt), leftCorners[i].y),
332  cv::Scalar( 0, 0, 255 ));
333 
334  ++inliers;
335  sumInliers += fabs(d-gt);
336 
337  if(fabs(d-gt) < 0.5f)
338  {
339  ++subInliers;
340  sumSubInliers += fabs(d-gt);
341  }
342  }
343  else if(mask.at<cv::Vec3b>(int(rightCorners[i].y), int(leftCorners[i].x))[0] == 255)
344  {
345  cv::line(left,
346  leftCorners[i],
347  cv::Point2f(leftCorners[i].x - gt, leftCorners[i].y),
348  cv::Scalar( 0, 255, 0 ));
349 
350  cv::line(left,
351  cv::Point2f(leftCorners[i].x - (d<gt?d:gt), leftCorners[i].y),
352  cv::Point2f(leftCorners[i].x - (d>gt?d:gt), leftCorners[i].y),
353  cv::Scalar( 0, 0, 255 ));
354  ++badInliers;
355  //UDEBUG("should be rejected or refined: %d pt=(%f,%f) (d=%f gt=%f)", i, leftCorners[i].x, leftCorners[i].y, d, gt);
356  }
357  else
358  {
359  cv::line(left,
360  leftCorners[i],
361  cv::Point2f(leftCorners[i].x - gt, leftCorners[i].y),
362  cv::Scalar( 255, 0, 0 ));
363 
364  cv::line(left,
365  cv::Point2f(leftCorners[i].x - (d<gt?d:gt), leftCorners[i].y),
366  cv::Point2f(leftCorners[i].x - (d>gt?d:gt), leftCorners[i].y),
367  cv::Scalar( 0, 0, 255 ));
368  ++badInliers;
369  //UDEBUG("should be rejected: %d", i);
370  }
371  }
372  else
373  {
374  ++badInliers;
375  }
376  }
377  else if(mask.at<cv::Vec3b>(int(rightCorners[i].y), int(leftCorners[i].x))[0] == 255 &&
378  rightCorners[i].x > 0.0f)
379  {
380  float d = leftCorners[i].x - rightCorners[i].x;
381  if(fabs(d-gt) < 1.0f)
382  {
383  cv::line(left,
384  leftCorners[i],
385  cv::Point2f(leftCorners[i].x - gt, leftCorners[i].y),
386  cv::Scalar( 0, 255, 255 ));
387 
388  cv::line(left,
389  cv::Point2f(leftCorners[i].x - (d<gt?d:gt), leftCorners[i].y),
390  cv::Point2f(leftCorners[i].x - (d>gt?d:gt), leftCorners[i].y),
391  cv::Scalar( 0, 0, 255 ));
392  ++goodRejected;
393  //UDEBUG("should not be rejected: %d", i);
394  }
395  else
396  {
397  ++badRejected;
398  }
399  }
400  else
401  {
402  ++badRejected;
403  //UDEBUG("correctly rejected: %d", i);
404  }
405  }
406 
407  UINFO("good accepted=%d (%d%%) bad accepted=%d (%d%%) good rejected=%d (%d%%) bad rejected=%d (%d%%)",
408  inliers,
409  (inliers*100)/leftCorners.size(),
410  badInliers,
411  (badInliers*100)/leftCorners.size(),
412  goodRejected,
413  (goodRejected*100)/leftCorners.size(),
414  badRejected,
415  (badRejected*100)/leftCorners.size());
416  UINFO("avg inliers =%f (subInliers=%f)", sumInliers/float(inliers), sumSubInliers/float(subInliers));
417 
418 
419  cv::namedWindow( "Right", cv::WINDOW_AUTOSIZE );
420  cv::imshow( "Right", right );
421 
422  cv::namedWindow( "Mask", cv::WINDOW_AUTOSIZE );
423  cv::imshow( "Mask", mask );
424 
425  cv::namedWindow( "Left", cv::WINDOW_AUTOSIZE );
426  cv::imshow( "Left", left );
427 
428  cv::waitKey(0);
429 
430  }
431 
432  return 0;
433 }
d
d
glm::mask
GLM_FUNC_DECL genIType mask(genIType const &count)
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:476
rtabmap::Parameters::getRemovedParameters
static const std::map< std::string, std::pair< bool, std::string > > & getRemovedParameters()
Definition: Parameters.cpp:229
UTimer
Definition: UTimer.h:46
uStr2Double
double UTILITE_EXP uStr2Double(const std::string &str)
Definition: UConversion.cpp:132
f
f
main
int main(int argc, char *argv[])
Definition: tools/StereoEval/main.cpp:55
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
Definition: Parameters.h:44
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:380
showUsage
void showUsage()
Definition: tools/StereoEval/main.cpp:46
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
UMath.h
Basic mathematics functions.
UConversion.h
Some conversion functions.
ULogger::setLevel
static void setLevel(ULogger::Level level)
Definition: ULogger.h:339
rtabmap::Feature2D::generateKeypoints
std::vector< cv::KeyPoint > generateKeypoints(const cv::Mat &image, const cv::Mat &mask=cv::Mat())
Definition: Features2d.cpp:657
uSplit
std::list< std::string > uSplit(const std::string &str, char separator= ' ')
Definition: UStl.h:566
uIsFinite
bool uIsFinite(const T &value)
Definition: UMath.h:55
UASSERT
#define UASSERT(condition)
UStl.h
Wrappers of STL for convenient functions.
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::Feature2D::create
static Feature2D * create(const ParametersMap &parameters=ParametersMap())
Definition: Features2d.cpp:503
ULogger::setType
static void setType(Type type, const std::string &fileName=kDefaultLogFileName, bool append=true)
Definition: ULogger.cpp:176
uReplaceChar
std::string UTILITE_EXP uReplaceChar(const std::string &str, char before, char after)
Definition: UConversion.cpp:32
uListToVector
std::vector< V > uListToVector(const std::list< V > &list)
Definition: UStl.h:475
rtabmap::Parameters::getDefaultParameters
static const ParametersMap & getDefaultParameters()
Definition: Parameters.h:778
UDEBUG
#define UDEBUG(...)
UERROR
#define UERROR(...)
rtabmap_oanet.argv
argv
Definition: rtabmap_oanet.py:12
ULogger.h
ULogger class and convenient macros.
UWARN
#define UWARN(...)
UTimer::ticks
double ticks()
Definition: UTimer.cpp:117
readPFM
cv::Mat readPFM(const char *filename)
Definition: io.h:99
uInsert
void uInsert(std::map< K, V > &map, const std::pair< K, V > &pair)
Definition: UStl.h:443
UINFO
#define UINFO(...)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:59