X-lanni
434 lines
12 KiB
C++
434 lines
12 KiB
C++
/*
|
|
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
|
All rights reserved.
|
|
|
|
Redistribution and use in source and binary forms, with or without
|
|
modification, are permitted provided that the following conditions are met:
|
|
* Redistributions of source code must retain the above copyright
|
|
notice, this list of conditions and the following disclaimer.
|
|
* Redistributions in binary form must reproduce the above copyright
|
|
notice, this list of conditions and the following disclaimer in the
|
|
documentation and/or other materials provided with the distribution.
|
|
* Neither the name of the Universite de Sherbrooke nor the
|
|
names of its contributors may be used to endorse or promote products
|
|
derived from this software without specific prior written permission.
|
|
|
|
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
|
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
|
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
|
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
|
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
|
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
|
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
|
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
|
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
|
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|
*/
|
|
|
|
#include "io.h"
|
|
#include <rtabmap/core/Parameters.h>
|
|
#include <rtabmap/core/Features2d.h>
|
|
#include <rtabmap/core/util2d.h>
|
|
#include <rtabmap/core/CameraModel.h>
|
|
#include <rtabmap/core/Stereo.h>
|
|
#include <rtabmap/core/StereoCameraModel.h>
|
|
#include <rtabmap/utilite/ULogger.h>
|
|
#include <rtabmap/utilite/UStl.h>
|
|
#include <rtabmap/utilite/UConversion.h>
|
|
#include <rtabmap/utilite/UTimer.h>
|
|
#include <rtabmap/utilite/UMath.h>
|
|
#include <opencv2/imgproc/types_c.h>
|
|
#include <fstream>
|
|
#include <string>
|
|
|
|
using namespace rtabmap;
|
|
|
|
void showUsage()
|
|
{
|
|
printf("Usage:\n"
|
|
"evalStereo.exe left.png right.png calib.txt disp.pfm mask.png [Parameters]\n"
|
|
"Example (with http://vision.middlebury.edu/stereo datasets):\n"
|
|
" $ ./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");
|
|
exit(1);
|
|
}
|
|
|
|
int main(int argc, char * argv[])
|
|
{
|
|
ULogger::setLevel(ULogger::kDebug);
|
|
ULogger::setType(ULogger::kTypeConsole);
|
|
|
|
if(argc < 6)
|
|
{
|
|
showUsage();
|
|
}
|
|
|
|
ParametersMap parameters = Parameters::getDefaultParameters();
|
|
for(int i=6; i<argc; ++i)
|
|
{
|
|
// Check for RTAB-Map's parameters
|
|
std::string key = argv[i];
|
|
key = uSplit(key, '-').back();
|
|
if(parameters.find(key) != parameters.end())
|
|
{
|
|
++i;
|
|
if(i < argc)
|
|
{
|
|
std::string value = argv[i];
|
|
if(value.empty())
|
|
{
|
|
showUsage();
|
|
}
|
|
else
|
|
{
|
|
value = uReplaceChar(value, ',', ' ');
|
|
}
|
|
std::pair<ParametersMap::iterator, bool> inserted = parameters.insert(ParametersPair(key, value));
|
|
if(inserted.second == false)
|
|
{
|
|
inserted.first->second = value;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
showUsage();
|
|
}
|
|
continue;
|
|
}
|
|
|
|
//backward compatibility
|
|
// look for old parameter name
|
|
std::map<std::string, std::pair<bool, std::string> >::const_iterator oldIter = Parameters::getRemovedParameters().find(key);
|
|
if(oldIter!=Parameters::getRemovedParameters().end())
|
|
{
|
|
++i;
|
|
if(i < argc)
|
|
{
|
|
std::string value = argv[i];
|
|
if(value.empty())
|
|
{
|
|
showUsage();
|
|
}
|
|
else
|
|
{
|
|
value = uReplaceChar(value, ',', ' ');
|
|
}
|
|
|
|
if(oldIter->second.first)
|
|
{
|
|
key = oldIter->second.second;
|
|
UWARN("Parameter migration from \"%s\" to \"%s\" (value=%s).",
|
|
oldIter->first.c_str(), oldIter->second.second.c_str(), value.c_str());
|
|
}
|
|
else if(oldIter->second.second.empty())
|
|
{
|
|
UERROR("Parameter \"%s\" doesn't exist anymore.", oldIter->first.c_str());
|
|
}
|
|
else
|
|
{
|
|
UERROR("Parameter \"%s\" doesn't exist anymore, check this similar parameter \"%s\".", oldIter->first.c_str(), oldIter->second.second.c_str());
|
|
}
|
|
if(oldIter->second.first)
|
|
{
|
|
std::pair<ParametersMap::iterator, bool> inserted = parameters.insert(ParametersPair(key, value));
|
|
if(inserted.second == false)
|
|
{
|
|
inserted.first->second = value;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
showUsage();
|
|
}
|
|
continue;
|
|
}
|
|
|
|
printf("Unrecognized option : %s\n", argv[i]);
|
|
showUsage();
|
|
}
|
|
|
|
UINFO("Loading files...");
|
|
|
|
cv::Mat left = cv::imread(argv[1]);
|
|
cv::Mat right = cv::imread(argv[2]);
|
|
cv::Mat disp = readPFM(argv[4]);
|
|
cv::Mat mask = cv::imread(argv[5]);
|
|
|
|
if(!left.empty() && !right.empty() && !disp.empty() && !mask.empty())
|
|
{
|
|
UASSERT(left.rows == disp.rows);
|
|
UASSERT(left.cols == disp.cols);
|
|
UASSERT(disp.rows == mask.rows);
|
|
UASSERT(disp.cols == mask.cols);
|
|
|
|
// read calib.txt
|
|
// Example format:
|
|
// --- calib.txt:
|
|
// cam0=[1038.018 0 322.037; 0 1038.018 243.393; 0 0 1]
|
|
// cam1=[1038.018 0 375.308; 0 1038.018 243.393; 0 0 1]
|
|
// doffs=53.271
|
|
// baseline=176.252
|
|
// width=718
|
|
// height=496
|
|
// ndisp=73
|
|
// isint=0
|
|
// vmin=8
|
|
// vmax=65
|
|
// dyavg=0.184
|
|
// dymax=0.423
|
|
// ---
|
|
std::string calibFile = argv[3];
|
|
std::ifstream stream(calibFile.c_str());
|
|
std::string line;
|
|
|
|
// two first lines are camera intrinsics
|
|
UINFO("Loading calibration... (%s)", calibFile.c_str());
|
|
std::vector<cv::Mat> K(2);
|
|
for(int i=0; i<2; ++i)
|
|
{
|
|
getline(stream, line);
|
|
line.erase(0, 6);
|
|
line = uReplaceChar(line, ']', "");
|
|
line = uReplaceChar(line, ';', "");
|
|
UINFO("K[%d] = %s", i, line.c_str());
|
|
std::vector<std::string> valuesStr = uListToVector(uSplit(line, ' '));
|
|
UASSERT(valuesStr.size() == 9);
|
|
K[i] = cv::Mat(3,3,CV_64FC1);
|
|
for(unsigned int j=0; j<valuesStr.size(); ++j)
|
|
{
|
|
K[i].at<double>(j) = uStr2Double(valuesStr[j]);
|
|
}
|
|
}
|
|
|
|
// skip doffs line
|
|
getline(stream, line);
|
|
|
|
// baseline
|
|
getline(stream, line);
|
|
line.erase(0, 9);
|
|
double baseline = uStr2Double(line);
|
|
UINFO("Baseline = %f", baseline);
|
|
|
|
StereoCameraModel model(
|
|
calibFile,
|
|
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)),
|
|
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)));
|
|
|
|
UASSERT(model.isValidForProjection());
|
|
|
|
UINFO("Processing...");
|
|
|
|
// Processing...
|
|
cv::Mat leftMono;
|
|
if(left.channels() == 3)
|
|
{
|
|
cv::cvtColor(left, leftMono, CV_BGR2GRAY);
|
|
}
|
|
else
|
|
{
|
|
leftMono = left;
|
|
}
|
|
cv::Mat rightMono;
|
|
if(right.channels() == 3)
|
|
{
|
|
cv::cvtColor(right, rightMono, CV_BGR2GRAY);
|
|
}
|
|
else
|
|
{
|
|
rightMono = right;
|
|
}
|
|
|
|
UTimer timer;
|
|
double timeKpts;
|
|
double timeSubPixel;
|
|
double timeStereo;
|
|
|
|
// generate kpts
|
|
std::vector<cv::KeyPoint> kpts;
|
|
uInsert(parameters, ParametersPair(Parameters::kKpRoiRatios(), "0.03 0.03 0.04 0.04"));
|
|
Feature2D * kptDetector = Feature2D::create(parameters);
|
|
kpts = kptDetector->generateKeypoints(leftMono);
|
|
delete kptDetector;
|
|
|
|
timeKpts = timer.ticks();
|
|
|
|
std::vector<cv::Point2f> leftCorners(kpts.size());
|
|
cv::KeyPoint::convert(kpts, leftCorners);
|
|
int subPixWinSize = 0;
|
|
int subPixIterations = 0;
|
|
double subPixEps = 0;
|
|
Parameters::parse(parameters, Parameters::kKpSubPixWinSize(), subPixWinSize);
|
|
Parameters::parse(parameters, Parameters::kKpSubPixIterations(), subPixIterations);
|
|
Parameters::parse(parameters, Parameters::kKpSubPixEps(), subPixEps);
|
|
if(subPixWinSize > 0 && subPixIterations > 0)
|
|
{
|
|
UDEBUG("cv::cornerSubPix() begin");
|
|
cv::cornerSubPix(leftMono, leftCorners,
|
|
cv::Size( subPixWinSize, subPixWinSize ),
|
|
cv::Size( -1, -1 ),
|
|
cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, subPixIterations, subPixEps ) );
|
|
UDEBUG("cv::cornerSubPix() end");
|
|
}
|
|
|
|
timeSubPixel = timer.ticks();
|
|
|
|
// Find features in the new right image
|
|
std::vector<unsigned char> status;
|
|
std::vector<cv::Point2f> rightCorners;
|
|
|
|
bool opticalFlow = false;
|
|
Parameters::parse(parameters, Parameters::kStereoOpticalFlow(), opticalFlow);
|
|
Stereo * stereo = 0;
|
|
if(opticalFlow)
|
|
{
|
|
stereo = new StereoOpticalFlow(parameters);
|
|
}
|
|
else
|
|
{
|
|
stereo = new Stereo(parameters);
|
|
}
|
|
|
|
rightCorners = stereo->computeCorrespondences(
|
|
leftMono,
|
|
rightMono,
|
|
leftCorners,
|
|
status);
|
|
delete stereo;
|
|
|
|
timeStereo = timer.ticks();
|
|
|
|
UINFO("Time: kpts:%f s, subpix=%f s, stereo=%f s", timeKpts, timeSubPixel, timeStereo);
|
|
|
|
UDEBUG("Mask = %d", mask.type());
|
|
|
|
int inliers = 0;
|
|
int subInliers = 0;
|
|
int badInliers = 0;
|
|
float sumInliers = 0.0f;
|
|
float sumSubInliers = 0.0f;
|
|
int goodRejected = 0;
|
|
int badRejected = 0;
|
|
for(unsigned int i=0; i<leftCorners.size(); ++i)
|
|
{
|
|
float gt = disp.at<float>(int(rightCorners[i].y), int(leftCorners[i].x));
|
|
if(status[i]!=0)
|
|
{
|
|
float d = leftCorners[i].x - rightCorners[i].x;
|
|
//float err = fabs(d-gt);
|
|
//UDEBUG("Pt(%f,%f): d=%f, gt=%f, error=%f", leftCorners[i].x, leftCorners[i].y, d, gt, err);
|
|
|
|
if(uIsFinite(gt))
|
|
{
|
|
if(fabs(d-gt) < 1.0f)
|
|
{
|
|
cv::line(left,
|
|
leftCorners[i],
|
|
cv::Point2f(leftCorners[i].x - gt, leftCorners[i].y),
|
|
cv::Scalar( 0, 255, 0 ));
|
|
|
|
cv::line(left,
|
|
cv::Point2f(leftCorners[i].x - (d<gt?d:gt), leftCorners[i].y),
|
|
cv::Point2f(leftCorners[i].x - (d>gt?d:gt), leftCorners[i].y),
|
|
cv::Scalar( 0, 0, 255 ));
|
|
|
|
++inliers;
|
|
sumInliers += fabs(d-gt);
|
|
|
|
if(fabs(d-gt) < 0.5f)
|
|
{
|
|
++subInliers;
|
|
sumSubInliers += fabs(d-gt);
|
|
}
|
|
}
|
|
else if(mask.at<cv::Vec3b>(int(rightCorners[i].y), int(leftCorners[i].x))[0] == 255)
|
|
{
|
|
cv::line(left,
|
|
leftCorners[i],
|
|
cv::Point2f(leftCorners[i].x - gt, leftCorners[i].y),
|
|
cv::Scalar( 0, 255, 0 ));
|
|
|
|
cv::line(left,
|
|
cv::Point2f(leftCorners[i].x - (d<gt?d:gt), leftCorners[i].y),
|
|
cv::Point2f(leftCorners[i].x - (d>gt?d:gt), leftCorners[i].y),
|
|
cv::Scalar( 0, 0, 255 ));
|
|
++badInliers;
|
|
//UDEBUG("should be rejected or refined: %d pt=(%f,%f) (d=%f gt=%f)", i, leftCorners[i].x, leftCorners[i].y, d, gt);
|
|
}
|
|
else
|
|
{
|
|
cv::line(left,
|
|
leftCorners[i],
|
|
cv::Point2f(leftCorners[i].x - gt, leftCorners[i].y),
|
|
cv::Scalar( 255, 0, 0 ));
|
|
|
|
cv::line(left,
|
|
cv::Point2f(leftCorners[i].x - (d<gt?d:gt), leftCorners[i].y),
|
|
cv::Point2f(leftCorners[i].x - (d>gt?d:gt), leftCorners[i].y),
|
|
cv::Scalar( 0, 0, 255 ));
|
|
++badInliers;
|
|
//UDEBUG("should be rejected: %d", i);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
++badInliers;
|
|
}
|
|
}
|
|
else if(mask.at<cv::Vec3b>(int(rightCorners[i].y), int(leftCorners[i].x))[0] == 255 &&
|
|
rightCorners[i].x > 0.0f)
|
|
{
|
|
float d = leftCorners[i].x - rightCorners[i].x;
|
|
if(fabs(d-gt) < 1.0f)
|
|
{
|
|
cv::line(left,
|
|
leftCorners[i],
|
|
cv::Point2f(leftCorners[i].x - gt, leftCorners[i].y),
|
|
cv::Scalar( 0, 255, 255 ));
|
|
|
|
cv::line(left,
|
|
cv::Point2f(leftCorners[i].x - (d<gt?d:gt), leftCorners[i].y),
|
|
cv::Point2f(leftCorners[i].x - (d>gt?d:gt), leftCorners[i].y),
|
|
cv::Scalar( 0, 0, 255 ));
|
|
++goodRejected;
|
|
//UDEBUG("should not be rejected: %d", i);
|
|
}
|
|
else
|
|
{
|
|
++badRejected;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
++badRejected;
|
|
//UDEBUG("correctly rejected: %d", i);
|
|
}
|
|
}
|
|
|
|
UINFO("good accepted=%d (%d%%) bad accepted=%d (%d%%) good rejected=%d (%d%%) bad rejected=%d (%d%%)",
|
|
inliers,
|
|
(inliers*100)/leftCorners.size(),
|
|
badInliers,
|
|
(badInliers*100)/leftCorners.size(),
|
|
goodRejected,
|
|
(goodRejected*100)/leftCorners.size(),
|
|
badRejected,
|
|
(badRejected*100)/leftCorners.size());
|
|
UINFO("avg inliers =%f (subInliers=%f)", sumInliers/float(inliers), sumSubInliers/float(subInliers));
|
|
|
|
|
|
cv::namedWindow( "Right", cv::WINDOW_AUTOSIZE );
|
|
cv::imshow( "Right", right );
|
|
|
|
cv::namedWindow( "Mask", cv::WINDOW_AUTOSIZE );
|
|
cv::imshow( "Mask", mask );
|
|
|
|
cv::namedWindow( "Left", cv::WINDOW_AUTOSIZE );
|
|
cv::imshow( "Left", left );
|
|
|
|
cv::waitKey(0);
|
|
|
|
}
|
|
|
|
return 0;
|
|
}
|