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  int outliers = 0;
308  float sumInliers = 0.0f;
309  float sumSubInliers = 0.0f;
310  int goodRejected = 0;
311  int badRejected = 0;
312  for(unsigned int i=0; i<leftCorners.size(); ++i)
313  {
314  float gt = disp.at<float>(int(rightCorners[i].y), int(leftCorners[i].x));
315  if(status[i]!=0)
316  {
317  float d = leftCorners[i].x - rightCorners[i].x;
318  //float err = fabs(d-gt);
319  //UDEBUG("Pt(%f,%f): d=%f, gt=%f, error=%f", leftCorners[i].x, leftCorners[i].y, d, gt, err);
320 
321  if(uIsFinite(gt))
322  {
323  if(fabs(d-gt) < 1.0f)
324  {
325  cv::line(left,
326  leftCorners[i],
327  cv::Point2f(leftCorners[i].x - gt, leftCorners[i].y),
328  cv::Scalar( 0, 255, 0 ));
329 
330  cv::line(left,
331  cv::Point2f(leftCorners[i].x - (d<gt?d:gt), leftCorners[i].y),
332  cv::Point2f(leftCorners[i].x - (d>gt?d:gt), leftCorners[i].y),
333  cv::Scalar( 0, 0, 255 ));
334 
335  ++inliers;
336  sumInliers += fabs(d-gt);
337 
338  if(fabs(d-gt) < 0.5f)
339  {
340  ++subInliers;
341  sumSubInliers += fabs(d-gt);
342  }
343  }
344  else if(mask.at<cv::Vec3b>(int(rightCorners[i].y), int(leftCorners[i].x))[0] == 255)
345  {
346  cv::line(left,
347  leftCorners[i],
348  cv::Point2f(leftCorners[i].x - gt, leftCorners[i].y),
349  cv::Scalar( 0, 255, 0 ));
350 
351  cv::line(left,
352  cv::Point2f(leftCorners[i].x - (d<gt?d:gt), leftCorners[i].y),
353  cv::Point2f(leftCorners[i].x - (d>gt?d:gt), leftCorners[i].y),
354  cv::Scalar( 0, 0, 255 ));
355  ++badInliers;
356  //UDEBUG("should be rejected or refined: %d pt=(%f,%f) (d=%f gt=%f)", i, leftCorners[i].x, leftCorners[i].y, d, gt);
357  }
358  else
359  {
360  cv::line(left,
361  leftCorners[i],
362  cv::Point2f(leftCorners[i].x - gt, leftCorners[i].y),
363  cv::Scalar( 255, 0, 0 ));
364 
365  cv::line(left,
366  cv::Point2f(leftCorners[i].x - (d<gt?d:gt), leftCorners[i].y),
367  cv::Point2f(leftCorners[i].x - (d>gt?d:gt), leftCorners[i].y),
368  cv::Scalar( 0, 0, 255 ));
369  ++outliers;
370  //UDEBUG("should be rejected: %d", i);
371  }
372  }
373  }
374  else if(mask.at<cv::Vec3b>(int(rightCorners[i].y), int(leftCorners[i].x))[0] == 255 &&
375  rightCorners[i].x > 0.0f)
376  {
377  float d = leftCorners[i].x - rightCorners[i].x;
378  cv::line(left,
379  leftCorners[i],
380  cv::Point2f(leftCorners[i].x - gt, leftCorners[i].y),
381  cv::Scalar( 0, 255, 255 ));
382 
383  cv::line(left,
384  cv::Point2f(leftCorners[i].x - (d<gt?d:gt), leftCorners[i].y),
385  cv::Point2f(leftCorners[i].x - (d>gt?d:gt), leftCorners[i].y),
386  cv::Scalar( 0, 0, 255 ));
387  ++goodRejected;
388  //UDEBUG("should not be rejected: %d", i);
389  }
390  else
391  {
392  ++badRejected;
393  //UDEBUG("correctly rejected: %d", i);
394  }
395  }
396 
397  UINFO("inliers=%d (%d%%) subInliers=%d (%d%%) bad inliers=%d (%d%%) bad accepted=%d (%d%%) good rejected=%d (%d%%) bad rejected=%d (%d%%)",
398  inliers,
399  (inliers*100)/leftCorners.size(),
400  subInliers,
401  (subInliers*100)/leftCorners.size(),
402  badInliers,
403  (badInliers*100)/leftCorners.size(),
404  outliers,
405  (outliers*100)/leftCorners.size(),
406  goodRejected,
407  (goodRejected*100)/leftCorners.size(),
408  badRejected,
409  (badRejected*100)/leftCorners.size());
410  UINFO("avg inliers =%f (subInliers=%f)", sumInliers/float(inliers), sumSubInliers/float(subInliers));
411 
412 
413  cv::namedWindow( "Right", cv::WINDOW_AUTOSIZE );
414  cv::imshow( "Right", right );
415 
416  cv::namedWindow( "Mask", cv::WINDOW_AUTOSIZE );
417  cv::imshow( "Mask", mask );
418 
419  cv::namedWindow( "Left", cv::WINDOW_AUTOSIZE );
420  cv::imshow( "Left", left );
421 
422  cv::waitKey(0);
423 
424  }
425 
426  return 0;
427 }
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:446
rtabmap::Parameters::getRemovedParameters
static const std::map< std::string, std::pair< bool, std::string > > & getRemovedParameters()
Definition: Parameters.cpp:224
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:42
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:364
rtabmap::Feature2D::generateKeypoints
std::vector< cv::KeyPoint > generateKeypoints(const cv::Mat &image, const cv::Mat &mask=cv::Mat()) const
Definition: Features2d.cpp:497
showUsage
void showUsage()
Definition: tools/StereoEval/main.cpp:46
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:41
UMath.h
Basic mathematics functions.
UConversion.h
Some conversion functions.
ULogger::setLevel
static void setLevel(ULogger::Level level)
Definition: ULogger.h:339
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:405
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:695
UDEBUG
#define UDEBUG(...)
UERROR
#define UERROR(...)
ULogger.h
ULogger class and convenient macros.
UWARN
#define UWARN(...)
UTimer::ticks
double ticks()
Definition: UTimer.cpp:110
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 Wed Jun 5 2019 22:41:31