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