00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include "io.h"
00029 #include <rtabmap/core/Parameters.h>
00030 #include <rtabmap/core/Features2d.h>
00031 #include <rtabmap/core/util2d.h>
00032 #include <rtabmap/core/CameraModel.h>
00033 #include <rtabmap/core/Stereo.h>
00034 #include <rtabmap/core/StereoCameraModel.h>
00035 #include <rtabmap/utilite/ULogger.h>
00036 #include <rtabmap/utilite/UStl.h>
00037 #include <rtabmap/utilite/UConversion.h>
00038 #include <rtabmap/utilite/UTimer.h>
00039 #include <rtabmap/utilite/UMath.h>
00040 #include <opencv2/imgproc/types_c.h>
00041 #include <fstream>
00042 #include <string>
00043
00044 using namespace rtabmap;
00045
00046 void showUsage()
00047 {
00048 printf("Usage:\n"
00049 "evalStereo.exe left.png right.png calib.txt disp.pfm mask.png [Parameters]\n"
00050 "Example (with http://vision.middlebury.edu/stereo datasets):\n"
00051 " $ ./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");
00052 exit(1);
00053 }
00054
00055 int main(int argc, char * argv[])
00056 {
00057 ULogger::setLevel(ULogger::kDebug);
00058 ULogger::setType(ULogger::kTypeConsole);
00059
00060 if(argc < 6)
00061 {
00062 showUsage();
00063 }
00064
00065 ParametersMap parameters = Parameters::getDefaultParameters();
00066 for(int i=6; i<argc; ++i)
00067 {
00068
00069 std::string key = argv[i];
00070 key = uSplit(key, '-').back();
00071 if(parameters.find(key) != parameters.end())
00072 {
00073 ++i;
00074 if(i < argc)
00075 {
00076 std::string value = argv[i];
00077 if(value.empty())
00078 {
00079 showUsage();
00080 }
00081 else
00082 {
00083 value = uReplaceChar(value, ',', ' ');
00084 }
00085 std::pair<ParametersMap::iterator, bool> inserted = parameters.insert(ParametersPair(key, value));
00086 if(inserted.second == false)
00087 {
00088 inserted.first->second = value;
00089 }
00090 }
00091 else
00092 {
00093 showUsage();
00094 }
00095 continue;
00096 }
00097
00098
00099
00100 std::map<std::string, std::pair<bool, std::string> >::const_iterator oldIter = Parameters::getRemovedParameters().find(key);
00101 if(oldIter!=Parameters::getRemovedParameters().end())
00102 {
00103 ++i;
00104 if(i < argc)
00105 {
00106 std::string value = argv[i];
00107 if(value.empty())
00108 {
00109 showUsage();
00110 }
00111 else
00112 {
00113 value = uReplaceChar(value, ',', ' ');
00114 }
00115
00116 if(oldIter->second.first)
00117 {
00118 key = oldIter->second.second;
00119 UWARN("Parameter migration from \"%s\" to \"%s\" (value=%s).",
00120 oldIter->first.c_str(), oldIter->second.second.c_str(), value.c_str());
00121 }
00122 else if(oldIter->second.second.empty())
00123 {
00124 UERROR("Parameter \"%s\" doesn't exist anymore.", oldIter->first.c_str());
00125 }
00126 else
00127 {
00128 UERROR("Parameter \"%s\" doesn't exist anymore, check this similar parameter \"%s\".", oldIter->first.c_str(), oldIter->second.second.c_str());
00129 }
00130 if(oldIter->second.first)
00131 {
00132 std::pair<ParametersMap::iterator, bool> inserted = parameters.insert(ParametersPair(key, value));
00133 if(inserted.second == false)
00134 {
00135 inserted.first->second = value;
00136 }
00137 }
00138 }
00139 else
00140 {
00141 showUsage();
00142 }
00143 continue;
00144 }
00145
00146 printf("Unrecognized option : %s\n", argv[i]);
00147 showUsage();
00148 }
00149
00150 UINFO("Loading files...");
00151
00152 cv::Mat left = cv::imread(argv[1]);
00153 cv::Mat right = cv::imread(argv[2]);
00154 cv::Mat disp = readPFM(argv[4]);
00155 cv::Mat mask = cv::imread(argv[5]);
00156
00157 if(!left.empty() && !right.empty() && !disp.empty() && !mask.empty())
00158 {
00159 UASSERT(left.rows == disp.rows);
00160 UASSERT(left.cols == disp.cols);
00161 UASSERT(disp.rows == mask.rows);
00162 UASSERT(disp.cols == mask.cols);
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180 std::string calibFile = argv[3];
00181 std::ifstream stream(calibFile.c_str());
00182 std::string line;
00183
00184
00185 UINFO("Loading calibration... (%s)", calibFile.c_str());
00186 std::vector<cv::Mat> K(2);
00187 for(int i=0; i<2; ++i)
00188 {
00189 getline(stream, line);
00190 line.erase(0, 6);
00191 line = uReplaceChar(line, ']', "");
00192 line = uReplaceChar(line, ';', "");
00193 UINFO("K[%d] = %s", i, line.c_str());
00194 std::vector<std::string> valuesStr = uListToVector(uSplit(line, ' '));
00195 UASSERT(valuesStr.size() == 9);
00196 K[i] = cv::Mat(3,3,CV_64FC1);
00197 for(unsigned int j=0; j<valuesStr.size(); ++j)
00198 {
00199 K[i].at<double>(j) = uStr2Double(valuesStr[j]);
00200 }
00201 }
00202
00203
00204 getline(stream, line);
00205
00206
00207 getline(stream, line);
00208 line.erase(0, 9);
00209 double baseline = uStr2Double(line);
00210 UINFO("Baseline = %f", baseline);
00211
00212 StereoCameraModel model(
00213 calibFile,
00214 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)),
00215 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)));
00216
00217 UASSERT(model.isValidForProjection());
00218
00219 UINFO("Processing...");
00220
00221
00222 cv::Mat leftMono;
00223 if(left.channels() == 3)
00224 {
00225 cv::cvtColor(left, leftMono, CV_BGR2GRAY);
00226 }
00227 else
00228 {
00229 leftMono = left;
00230 }
00231 cv::Mat rightMono;
00232 if(right.channels() == 3)
00233 {
00234 cv::cvtColor(right, rightMono, CV_BGR2GRAY);
00235 }
00236 else
00237 {
00238 rightMono = right;
00239 }
00240
00241 UTimer timer;
00242 double timeKpts;
00243 double timeSubPixel;
00244 double timeStereo;
00245
00246
00247 std::vector<cv::KeyPoint> kpts;
00248 uInsert(parameters, ParametersPair(Parameters::kKpRoiRatios(), "0.03 0.03 0.04 0.04"));
00249 Feature2D * kptDetector = Feature2D::create(parameters);
00250 kpts = kptDetector->generateKeypoints(leftMono);
00251 delete kptDetector;
00252
00253 timeKpts = timer.ticks();
00254
00255 std::vector<cv::Point2f> leftCorners(kpts.size());
00256 cv::KeyPoint::convert(kpts, leftCorners);
00257 int subPixWinSize = 0;
00258 int subPixIterations = 0;
00259 double subPixEps = 0;
00260 Parameters::parse(parameters, Parameters::kKpSubPixWinSize(), subPixWinSize);
00261 Parameters::parse(parameters, Parameters::kKpSubPixIterations(), subPixIterations);
00262 Parameters::parse(parameters, Parameters::kKpSubPixEps(), subPixEps);
00263 if(subPixWinSize > 0 && subPixIterations > 0)
00264 {
00265 UDEBUG("cv::cornerSubPix() begin");
00266 cv::cornerSubPix(leftMono, leftCorners,
00267 cv::Size( subPixWinSize, subPixWinSize ),
00268 cv::Size( -1, -1 ),
00269 cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, subPixIterations, subPixEps ) );
00270 UDEBUG("cv::cornerSubPix() end");
00271 }
00272
00273 timeSubPixel = timer.ticks();
00274
00275
00276 std::vector<unsigned char> status;
00277 std::vector<cv::Point2f> rightCorners;
00278
00279 bool opticalFlow = false;
00280 Parameters::parse(parameters, Parameters::kStereoOpticalFlow(), opticalFlow);
00281 Stereo * stereo = 0;
00282 if(opticalFlow)
00283 {
00284 stereo = new StereoOpticalFlow(parameters);
00285 }
00286 else
00287 {
00288 stereo = new Stereo(parameters);
00289 }
00290
00291 rightCorners = stereo->computeCorrespondences(
00292 leftMono,
00293 rightMono,
00294 leftCorners,
00295 status);
00296 delete stereo;
00297
00298 timeStereo = timer.ticks();
00299
00300 UINFO("Time: kpts:%f s, subpix=%f s, stereo=%f s", timeKpts, timeSubPixel, timeStereo);
00301
00302 UDEBUG("Mask = %d", mask.type());
00303
00304 int inliers = 0;
00305 int subInliers = 0;
00306 int badInliers = 0;
00307 int outliers = 0;
00308 float sumInliers = 0.0f;
00309 float sumSubInliers = 0.0f;
00310 int goodRejected = 0;
00311 int badRejected = 0;
00312 for(unsigned int i=0; i<leftCorners.size(); ++i)
00313 {
00314 float gt = disp.at<float>(int(rightCorners[i].y), int(leftCorners[i].x));
00315 if(status[i]!=0)
00316 {
00317 float d = leftCorners[i].x - rightCorners[i].x;
00318
00319
00320
00321 if(uIsFinite(gt))
00322 {
00323 if(fabs(d-gt) < 1.0f)
00324 {
00325 cv::line(left,
00326 leftCorners[i],
00327 cv::Point2f(leftCorners[i].x - gt, leftCorners[i].y),
00328 cv::Scalar( 0, 255, 0 ));
00329
00330 cv::line(left,
00331 cv::Point2f(leftCorners[i].x - (d<gt?d:gt), leftCorners[i].y),
00332 cv::Point2f(leftCorners[i].x - (d>gt?d:gt), leftCorners[i].y),
00333 cv::Scalar( 0, 0, 255 ));
00334
00335 ++inliers;
00336 sumInliers += fabs(d-gt);
00337
00338 if(fabs(d-gt) < 0.5f)
00339 {
00340 ++subInliers;
00341 sumSubInliers += fabs(d-gt);
00342 }
00343 }
00344 else if(mask.at<cv::Vec3b>(int(rightCorners[i].y), int(leftCorners[i].x))[0] == 255)
00345 {
00346 cv::line(left,
00347 leftCorners[i],
00348 cv::Point2f(leftCorners[i].x - gt, leftCorners[i].y),
00349 cv::Scalar( 0, 255, 0 ));
00350
00351 cv::line(left,
00352 cv::Point2f(leftCorners[i].x - (d<gt?d:gt), leftCorners[i].y),
00353 cv::Point2f(leftCorners[i].x - (d>gt?d:gt), leftCorners[i].y),
00354 cv::Scalar( 0, 0, 255 ));
00355 ++badInliers;
00356
00357 }
00358 else
00359 {
00360 cv::line(left,
00361 leftCorners[i],
00362 cv::Point2f(leftCorners[i].x - gt, leftCorners[i].y),
00363 cv::Scalar( 255, 0, 0 ));
00364
00365 cv::line(left,
00366 cv::Point2f(leftCorners[i].x - (d<gt?d:gt), leftCorners[i].y),
00367 cv::Point2f(leftCorners[i].x - (d>gt?d:gt), leftCorners[i].y),
00368 cv::Scalar( 0, 0, 255 ));
00369 ++outliers;
00370
00371 }
00372 }
00373 }
00374 else if(mask.at<cv::Vec3b>(int(rightCorners[i].y), int(leftCorners[i].x))[0] == 255 &&
00375 rightCorners[i].x > 0.0f)
00376 {
00377 float d = leftCorners[i].x - rightCorners[i].x;
00378 cv::line(left,
00379 leftCorners[i],
00380 cv::Point2f(leftCorners[i].x - gt, leftCorners[i].y),
00381 cv::Scalar( 0, 255, 255 ));
00382
00383 cv::line(left,
00384 cv::Point2f(leftCorners[i].x - (d<gt?d:gt), leftCorners[i].y),
00385 cv::Point2f(leftCorners[i].x - (d>gt?d:gt), leftCorners[i].y),
00386 cv::Scalar( 0, 0, 255 ));
00387 ++goodRejected;
00388
00389 }
00390 else
00391 {
00392 ++badRejected;
00393
00394 }
00395 }
00396
00397 UINFO("inliers=%d (%d%%) subInliers=%d (%d%%) bad inliers=%d (%d%%) bad accepted=%d (%d%%) good rejected=%d (%d%%) bad rejected=%d (%d%%)",
00398 inliers,
00399 (inliers*100)/leftCorners.size(),
00400 subInliers,
00401 (subInliers*100)/leftCorners.size(),
00402 badInliers,
00403 (badInliers*100)/leftCorners.size(),
00404 outliers,
00405 (outliers*100)/leftCorners.size(),
00406 goodRejected,
00407 (goodRejected*100)/leftCorners.size(),
00408 badRejected,
00409 (badRejected*100)/leftCorners.size());
00410 UINFO("avg inliers =%f (subInliers=%f)", sumInliers/float(inliers), sumSubInliers/float(subInliers));
00411
00412
00413 cv::namedWindow( "Right", cv::WINDOW_AUTOSIZE );
00414 cv::imshow( "Right", right );
00415
00416 cv::namedWindow( "Mask", cv::WINDOW_AUTOSIZE );
00417 cv::imshow( "Mask", mask );
00418
00419 cv::namedWindow( "Left", cv::WINDOW_AUTOSIZE );
00420 cv::imshow( "Left", left );
00421
00422 cv::waitKey(0);
00423
00424 }
00425
00426 return 0;
00427 }