i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-01 17:10:26 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added Monocular SLAM (experimental), Odometry classes refactoring

Browse Source
This commit is contained in:
Mathieu Labbe
2015-04-23 22:01:41 -04:00
parent 39dce825d6
commit fa3a2421f6
39 changed files with 4138 additions and 2702 deletions
Download Patch File Download Diff File Expand all files Collapse all files

564
tools/EpipolarGeometry/main.cpp
View File

@@ -63,570 +63,6 @@ void showUsage()
exit(1);
}
class RTABMAP_EXP OdometryMono : public Odometry
{
public:
OdometryMono(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap()) :
Odometry(parameters),
flowWinSize_(Parameters::defaultOdomFlowWinSize()),
flowIterations_(Parameters::defaultOdomFlowIterations()),
flowEps_(Parameters::defaultOdomFlowEps()),
flowMaxLevel_(Parameters::defaultOdomFlowMaxLevel()),
subPixWinSize_(Parameters::defaultOdomSubPixWinSize()),
subPixIterations_(Parameters::defaultOdomSubPixIterations()),
subPixEps_(Parameters::defaultOdomSubPixEps()),
refCorners3D_(new pcl::PointCloud<pcl::PointXYZ>)
{
Parameters::parse(parameters, Parameters::kOdomFlowWinSize(), flowWinSize_);
Parameters::parse(parameters, Parameters::kOdomFlowIterations(), flowIterations_);
Parameters::parse(parameters, Parameters::kOdomFlowEps(), flowEps_);
Parameters::parse(parameters, Parameters::kOdomFlowMaxLevel(), flowMaxLevel_);
Parameters::parse(parameters, Parameters::kOdomSubPixWinSize(), subPixWinSize_);
Parameters::parse(parameters, Parameters::kOdomSubPixIterations(), subPixIterations_);
Parameters::parse(parameters, Parameters::kOdomSubPixEps(), subPixEps_);
ParametersMap::const_iterator iter;
Feature2D::Type detectorStrategy = (Feature2D::Type)Parameters::defaultOdomFeatureType();
if((iter=parameters.find(Parameters::kOdomFeatureType())) != parameters.end())
{
detectorStrategy = (Feature2D::Type)std::atoi((*iter).second.c_str());
}
feature2D_ = Feature2D::create(detectorStrategy, parameters);
ParametersMap customParameters;
customParameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uValue(parameters, Parameters::kOdomBowNNType(), uNumber2Str(Parameters::defaultOdomBowNNType()))));
customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uValue(parameters, Parameters::kOdomBowNNDR(), uNumber2Str(Parameters::defaultOdomBowNNDR()))));
customParameters.insert(ParametersPair(Parameters::kKpNewWordsComparedTogether(), "false"));
dictionary_ = new VWDictionary(customParameters);
}
virtual ~OdometryMono()
{
delete feature2D_;
delete dictionary_;
}
private:
virtual Transform computeTransform(const SensorData & data, OdometryInfo * info = 0)
{
UTimer timer;
Transform output;
int inliers = 0;
int correspondences = 0;
cv::Mat newFrame;
// convert to grayscale
if(data.image().channels() > 1)
{
cv::cvtColor(data.image(), newFrame, cv::COLOR_BGR2GRAY);
}
else
{
newFrame = data.image().clone();
}
UDEBUG("lastCorners_.size()=%d lastFrame_=%d", (int)refCorners_.size(), refFrame_.empty()?0:1);
if(!refFrame_.empty() && refCorners_.size())
{
if(refCorners3D_->size())
{
//PnP
UDEBUG("PnP");
std::vector<cv::KeyPoint> newKpts;
std::vector<cv::Point2f> newCorners;
cv::Mat newDescriptors;
if(data.keypoints().size())
{
cv::KeyPoint::convert(data.keypoints(), newCorners);
newKpts = data.keypoints();
newDescriptors = data.descriptors();
}
else
{
// generate kpts
cv::Rect roi = Feature2D::computeRoi(newFrame, this->getRoiRatios());
newKpts = feature2D_->generateKeypoints(newFrame, this->getMaxFeatures(), roi);
Feature2D::limitKeypoints(newKpts, this->getMaxFeatures());
if(newKpts.size())
{
//extract descriptors (before subpixel)
newDescriptors = feature2D_->generateDescriptors(newFrame, newKpts);
cv::KeyPoint::convert(newKpts, newCorners);
if(subPixWinSize_ > 0 && subPixIterations_ > 0)
{
UDEBUG("cv::cornerSubPix() begin");
cv::cornerSubPix(newFrame, newCorners,
cv::Size( subPixWinSize_, subPixWinSize_ ),
cv::Size( -1, -1 ),
cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, subPixIterations_, subPixEps_ ) );
UDEBUG("cv::cornerSubPix() end");
for(unsigned int i=0; i<newCorners.size(); ++i)
{
newKpts[i].pt = newCorners[i];
}
}
}
}
//matching using visual words dictionary
std::vector<int> newWordIds = uListToVector(dictionary_->addNewWords(newDescriptors, 2));
UDEBUG("");
UASSERT((int)newKpts.size() == newDescriptors.rows);
UASSERT(newKpts.size() == newWordIds.size());
std::multimap<int, cv::KeyPoint> newWords;
for(unsigned int i=0; i<newWordIds.size(); ++i)
{
newWords.insert(std::make_pair(newWordIds[i], newKpts[i]));
}
UDEBUG("newWords=%d", (int)newWords.size());
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
if(EpipolarGeometry::findPairsUnique(refWords_, newWords, pairs) > this->getMinInliers())
{
UDEBUG("pairs = %d", (int)pairs.size());
// now that we have correspondences, set data for PnP
std::vector<cv::Point3f> objectPoints(pairs.size());
std::vector<cv::Point2f> imagePoints(pairs.size());
int i=0;
std::vector<cv::KeyPoint> a,b;
for(std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > >::iterator iter = pairs.begin();
iter!=pairs.end();
++iter)
{
pcl::PointXYZ pt3 = refCorners3D_->at(iter->first-1); // id and index should match
objectPoints[i] = cv::Point3f(pt3.x, pt3.y, pt3.z);
imagePoints[i] = iter->second.second.pt;
UDEBUG("ref (%f %f) new (%f %f) pt (%f %f %f)",
iter->second.first.pt.x, iter->second.first.pt.y,
iter->second.second.pt.x, iter->second.second.pt.y,
pt3.x, pt3.y, pt3.z);
a.push_back(iter->second.first);
b.push_back(iter->second.second);
++i;
}
UDEBUG("");
cv::Mat K = (cv::Mat_<double>(3,3) <<
data.fx(), 0, data.cx(),
0, data.fyOrBaseline(), data.cy(),
0, 0, 1);
cv::Mat rvec, tvec;
std::vector<int> inliers;
cv::solvePnPRansac(objectPoints, imagePoints, K, cv::Mat(), rvec, tvec, false, 100, 8., 100, inliers);
UDEBUG("");
UDEBUG("inliers=%d/%d", (int)inliers.size(), (int)objectPoints.size());
/*
/// Debug draw matches
std::vector<cv::DMatch> good_matches(inliers.size());
for(i=0; i<(int)good_matches.size(); ++i)
{
good_matches[i].trainIdx = inliers[i];
good_matches[i].queryIdx = inliers[i];
}
cv::Mat imgInliers;
cv::drawMatches( refFrame_, a, newFrame, b,
good_matches, imgInliers, cv::Scalar::all(-1), cv::Scalar::all(-1),
std::vector<char>(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );
UWARN("saved test.png");
cv::imwrite("test.png", imgInliers);
cv::imwrite("testa.png", refFrame_);
cv::imwrite("testb.png", newFrame);
/// Debug draw matches
*/
if((int)inliers.size() > this->getMinInliers())
{
cv::Mat R(3,3,CV_64FC1);
cv::Rodrigues(rvec, R);
std::cout << "R: " << R << std::endl;
std::cout << "T: " << tvec << std::endl;
//R = R.t(); // rotation of inverse
//tvec = -R * tvec; // translation of inverse
//UDEBUG("camera movement:");
//std::cout << "R: " << R << std::endl;
//std::cout << "T: " << tvec << std::endl;
output = Transform(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvec.at<double>(0),
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
output = data.localTransform() * output.inverse() * data.localTransform().inverse();
output = this->getPose().inverse() * refCorners3DPose_ * output;
}
else
{
UWARN("PnP not enough inliers (%d < %d), rejecting the transform...", (int)inliers.size(), this->getMinInliers());
}
}
else
{
UWARN("Not enough pairs found (%d)...", (int)pairs.size());
}
// remove new words from dictionary
for(unsigned int i=0; i<newWordIds.size(); ++i)
{
dictionary_->removeAllWordRef(newWordIds[i], 2);
}
dictionary_->deleteUnusedWords();
}
else
{
//flow
UDEBUG("flow");
// Find features in the new left image
std::vector<unsigned char> status;
std::vector<float> err;
std::vector<cv::Point2f> flowCorners = refCornersGuess_;
std::vector<cv::Point2f> refCorners = refCorners_;
std::vector<cv::KeyPoint> refKpts = refKpts_;
cv::Mat refDescriptors = refDescriptors_;
UDEBUG("cv::calcOpticalFlowPyrLK() begin (ref=%d guess=%d)", (int)refCorners.size(), (int)flowCorners.size());
cv::calcOpticalFlowPyrLK(
refFrame_,
newFrame,
refCorners,
flowCorners,
status,
err,
cv::Size(flowWinSize_, flowWinSize_), flowMaxLevel_,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations_, flowEps_),
cv::OPTFLOW_LK_GET_MIN_EIGENVALS | cv::OPTFLOW_USE_INITIAL_FLOW, 1e-4);
UDEBUG("cv::calcOpticalFlowPyrLK() end");
UDEBUG("Filtering optical flow outliers...");
std::vector<cv::Point2f> tmpFlowCorners(status.size());
std::vector<cv::Point2f> tmpRefCorners(status.size());
std::vector<cv::KeyPoint> tmpRefKpts(status.size());
cv::Mat tmpRefDescriptors;
int oi = 0;
float flow = 0;
float minFlow = 50;
UASSERT(flowCorners.size() == status.size());
UASSERT(refCorners.size() == status.size());
UASSERT(refKpts.size() == status.size());
UASSERT(refDescriptors.rows == (int)status.size());
for(unsigned int i=0; i<status.size(); ++i)
{
if(status[i] && refCornersMask_[i])
{
float dx = refCorners[i].x - flowCorners[i].x;
float dy = refCorners[i].y - flowCorners[i].y;
float tmp = std::sqrt(dx*dx + dy*dy);
flow+=tmp;
tmpFlowCorners[oi] = flowCorners[i];
tmpRefCorners[oi] = refCorners[i];
tmpRefKpts[oi] = refKpts[i];
tmpRefDescriptors.push_back(refDescriptors.row(i));
++oi;
UDEBUG("%d = ref(%f %f) flow(%f %f) = %f", i,
refCorners[i].x, refCorners[i].y,
flowCorners[i].x, flowCorners[i].y,
tmp);
}
else
{
refCornersMask_[i] = 0;
}
}
if(oi)
{
flow /=float(oi);
}
tmpFlowCorners.resize(oi);
tmpRefCorners.resize(oi);
tmpRefKpts.resize(oi);
UDEBUG("Filtering optical flow outliers...done! (inliers=%d/%d)", oi, (int)status.size());
if(flow > minFlow && oi > this->getMinInliers())
{
flowCorners = tmpFlowCorners;
refCorners = tmpRefCorners;
refKpts = tmpRefKpts;
refDescriptors = tmpRefDescriptors;
UDEBUG("flow=%f", flow);
// compute fundamental matrix
UDEBUG("Find fundamental matrix");
status.clear();
cv::Mat F = cv::findFundamentalMat(refCorners, flowCorners, status, cv::RANSAC, 3.0, 0.99);
std::cout << "F=" << F << std::endl;
if(!F.empty())
{
UDEBUG("Filtering fundamental matrix outliers...");
tmpFlowCorners.resize(status.size());
tmpRefCorners.resize(status.size());
tmpRefKpts.resize(status.size());
tmpRefDescriptors = cv::Mat();
oi = 0;
UASSERT(flowCorners.size() == status.size());
UASSERT(refCorners.size() == status.size());
UASSERT(refKpts.size() == status.size());
UASSERT(refDescriptors.rows == (int)status.size());
for(unsigned int i=0; i<status.size(); ++i)
{
if(status[i])
{
tmpFlowCorners[oi] = flowCorners[i];
tmpRefCorners[oi] = refCorners[i];
tmpRefKpts[oi] = refKpts[i];
tmpRefDescriptors.push_back(refDescriptors.row(i));
++oi;
}
}
tmpFlowCorners.resize(oi);
tmpRefCorners.resize(oi);
tmpRefKpts.resize(oi);
flowCorners = tmpFlowCorners;
refCorners = tmpRefCorners;
refKpts = tmpRefKpts;
refDescriptors = tmpRefDescriptors;
UDEBUG("Filtering fundamental matrix outliers...done! (inliers=%d/%d)", oi, (int)status.size());
if(refCorners.size())
{
std::vector<cv::Point2f> lastCornersRefined;
std::vector<cv::Point2f> newCornersRefined;
//UDEBUG("Correcting matches...");
cv::correctMatches(F, refCorners, flowCorners, lastCornersRefined, newCornersRefined);
refCorners = lastCornersRefined;
flowCorners = newCornersRefined;
//UDEBUG("Correcting matches...done!");
UDEBUG("Computing P...");
cv::Mat K = (cv::Mat_<double>(3,3) <<
data.fx(), 0, data.cx(),
0, data.fyOrBaseline(), data.cy(),
0, 0, 1);
//std::cout << "K=" << K << std::endl;
cv::Mat Kinv = K.inv();
//std::cout << "Kinv=" << Kinv << std::endl;
cv::Mat E = K.t()*F*K;
std::cout << "E=" << E << std::endl;
//normalize coordinates
cv::Mat x(3, refCorners.size(), CV_64FC1);
cv::Mat xp(3, refCorners.size(), CV_64FC1);
for(unsigned int i=0; i<refCorners.size(); ++i)
{
x.at<double>(0, i) = refCorners[i].x;
x.at<double>(1, i) = refCorners[i].y;
x.at<double>(2, i) = 1;
xp.at<double>(0, i) = flowCorners[i].x;
xp.at<double>(1, i) = flowCorners[i].y;
xp.at<double>(2, i) = 1;
//UDEBUG("ptA= %f %f %f", ptA.at<double>(0, i), ptA.at<double>(1, i), ptA.at<double>(2, i));
}
cv::Mat x_norm = Kinv * x;
cv::Mat xp_norm = Kinv * xp;
x_norm = x_norm.rowRange(0,2);
xp_norm = xp_norm.rowRange(0,2);
x = x.rowRange(0,2);
xp = xp.rowRange(0,2);
cv::Mat P = EpipolarGeometry::findPFromE(E, x_norm, xp_norm);
if(!P.empty())
{
cv::Mat P0 = cv::Mat::zeros(3, 4, CV_64FC1);
P0.at<double>(0,0) = 1;
P0.at<double>(1,1) = 1;
P0.at<double>(2,2) = 1;
UDEBUG("Computing P...done!");
std::cout << "P=" << P << std::endl;
//scale
//P.col(3) /= 10.0;
cv::Mat R, T;
EpipolarGeometry::findRTFromP(P, R, T);
//std::cout << "R=" << R << std::endl;
//std::cout << "T=" << T << std::endl;
UDEBUG("");
//cv::Mat pts4D;
std::vector<double> reprojErrors;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
EpipolarGeometry::triangulatePoints(x_norm, xp_norm, P0, P, cloud, reprojErrors);
//cv::triangulatePoints(P0, P, x_norm, xp_norm, pts4D);
tmpRefCorners.resize(cloud->size());
tmpRefKpts.resize(cloud->size());
tmpRefDescriptors = cv::Mat();
refCorners3D_->resize(cloud->size());
oi = 0;
UASSERT(refCorners.size() == cloud->size());
UASSERT(refKpts.size() == cloud->size());
UASSERT(refDescriptors.rows == (int)cloud->size());
for(unsigned int i=0; i<cloud->size(); ++i)
{
if(cloud->at(i).z>0)
{
refCorners3D_->at(oi) = cloud->at(i);
tmpRefCorners[oi] = refCorners[i];
tmpRefKpts[oi] = refKpts[i];
tmpRefDescriptors.push_back(refDescriptors.row(i));
++oi;
}
}
refCorners3D_->resize(oi);
tmpRefCorners.resize(oi);
tmpRefKpts.resize(oi);
refCorners = tmpRefCorners;
refKpts = tmpRefKpts;
refDescriptors = tmpRefDescriptors;
UDEBUG("Filtering triangulation outliers...done! (inliers=%d/%d)", oi, (int)cloud->size());
//refCorners3D_ = util3d::transformPointCloud<pcl::PointXYZ>(refCorners3D_, data.localTransform());
refCorners3DPose_ = this->getPose();
dictionary_->clear();
refCorners_ = refCorners;
refKpts_ = refKpts;
refDescriptors_ = refDescriptors;
std::vector<int> wordsId = uListToVector(dictionary_->addNewWords(refDescriptors_, 1));
refWords_.clear();
UASSERT(wordsId.size() == refCorners_.size());
for(unsigned int i=0; i<wordsId.size(); ++i)
{
refWords_.insert(std::make_pair(wordsId[i], refKpts[i]));
}
dictionary_->update();
output = Transform(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), T.at<double>(0)/*/T.at<double>(3)*/,
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), T.at<double>(1)/*/T.at<double>(3)*/,
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), T.at<double>(2)/*/T.at<double>(3)*/);
output = data.localTransform() * output.inverse() * data.localTransform().inverse();
}
else
{
UFATAL("No valid camera matrix found!");
}
}
}
}
else
{
UWARN("Flow not enough high! flow=%f ki=%d", flow, oi);
refCornersGuess_ = flowCorners;
}
}
}
else
{
//return Identity
output = Transform::getIdentity();
std::vector<cv::KeyPoint> newKpts;
std::vector<cv::Point2f> newCorners;
cv::Mat newDescriptors;
if(data.keypoints().size())
{
cv::KeyPoint::convert(data.keypoints(), newCorners);
newKpts = data.keypoints();
newDescriptors = data.descriptors();
}
else
{
// generate kpts
cv::Rect roi = Feature2D::computeRoi(newFrame, this->getRoiRatios());
newKpts = feature2D_->generateKeypoints(newFrame, this->getMaxFeatures(), roi);
Feature2D::limitKeypoints(newKpts, this->getMaxFeatures());
if(newKpts.size())
{
//extract descriptors (before subpixel)
newDescriptors = feature2D_->generateDescriptors(newFrame, newKpts);
cv::KeyPoint::convert(newKpts, newCorners);
if(subPixWinSize_ > 0 && subPixIterations_ > 0)
{
UDEBUG("cv::cornerSubPix() begin");
cv::cornerSubPix(newFrame, newCorners,
cv::Size( subPixWinSize_, subPixWinSize_ ),
cv::Size( -1, -1 ),
cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, subPixIterations_, subPixEps_ ) );
UDEBUG("cv::cornerSubPix() end");
for(unsigned int i=0; i<newCorners.size(); ++i)
{
newKpts[i].pt = newCorners[i];
}
}
}
}
if((int)newCorners.size() > this->getMinInliers())
{
refFrame_ = newFrame;
refCorners_ = newCorners;
refKpts_ = newKpts;
refDescriptors_ = newDescriptors;
refCornersGuess_ = newCorners;
refCornersMask_.resize(newCorners.size(), 1);
UASSERT(refCorners_.size() == refKpts_.size());
UASSERT(refDescriptors_.rows == (int)refKpts_.size());
}
else
{
UWARN("Too low 2D corners (%d), ignoring new frame...",
(int)newCorners.size());
}
}
UINFO("Odom update time = %fs tf=[%s] inliers=%d/%d, transform accepted=%s",
timer.elapsed(),
output.prettyPrint().c_str(),
inliers,
correspondences,
!output.isNull()?"true":"false");
return output;
}
private:
//Parameters:
int flowWinSize_;
int flowIterations_;
double flowEps_;
int flowMaxLevel_;
int subPixWinSize_;
int subPixIterations_;
double subPixEps_;
Feature2D * feature2D_;
VWDictionary * dictionary_;
cv::Mat refFrame_;
std::vector<cv::Point2f> refCorners_;
std::vector<cv::Point2f> refCornersGuess_;
std::vector<unsigned char> refCornersMask_;
pcl::PointCloud<pcl::PointXYZ>::Ptr refCorners3D_;
Transform refCorners3DPose_;
std::vector<cv::KeyPoint> refKpts_;
cv::Mat refDescriptors_;
std::multimap<int, cv::KeyPoint> refWords_;
};
class MainWidget : public QWidget
{
public:

121
tools/OdometryViewer/main.cpp
View File

@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/core/Odometry.h>
#include <rtabmap/core/OdometryThread.h>
#include <rtabmap/gui/OdometryViewer.h>
#include <rtabmap/core/CameraThread.h>
#include <rtabmap/core/CameraRGBD.h>
@@ -48,8 +49,9 @@ void showUsage()
" -o # Odometry type (default 6): 0=SURF, 1=SIFT, 2=ORB, 3=FAST/FREAK, 4=FAST/BRIEF, 5=GFTT/FREAK, 6=GFTT/BRIEF, 7=BRISK\n"
" -nn # Nearest neighbor strategy (default 3): kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4\n"
" -nndr # Nearest neighbor distance ratio (default 0.7)\n"
" -icp Use ICP odometry\n"
" -flow Use optical flow odometry.\n"
" -icp Use ICP odometry\n"
" -mono Use Mono odometry\n"
"\n"
" -hz #.# Camera rate (default 0, 0 means as fast as the camera can)\n"
" -db \"input.db\" Use database instead of camera (recorded with rtabmap-dataRecorder)\n"
@@ -97,28 +99,29 @@ int main (int argc, char * argv[])
float rate = 0.0;
std::string inputDatabase;
int driver = 0;
int odomType = 6;
int odomType = rtabmap::Parameters::defaultOdomFeatureType();
bool icp = false;
bool flow = false;
int nnType =3;
float nndr = 0.7f;
float distance = 0.01;
int maxWords = 0;
int minInliers = 20;
float maxDepth = 5.0f;
int iterations = 30;
int resetCountdown = 0;
bool mono = false;
int nnType = rtabmap::Parameters::defaultOdomBowNNType();
float nndr = rtabmap::Parameters::defaultOdomBowNNDR();
float distance = rtabmap::Parameters::defaultOdomInlierDistance();
int maxWords = rtabmap::Parameters::defaultOdomMaxFeatures();
int minInliers = rtabmap::Parameters::defaultOdomMinInliers();
float maxDepth = rtabmap::Parameters::defaultOdomMaxDepth();
int iterations = rtabmap::Parameters::defaultOdomIterations();
int resetCountdown = rtabmap::Parameters::defaultOdomResetCountdown();
int decimation = 4;
float voxel = 0.005;
int samples = 10000;
float ratio = 0.7f;
int maxClouds = 10;
int briefBytes = 32;
int fastThr = 30;
int briefBytes = rtabmap::Parameters::defaultBRIEFBytes();
int fastThr = rtabmap::Parameters::defaultFASTThreshold();
float sec = 0.0f;
bool gpu = false;
int localHistory = 1000;
bool p2p = false;
int localHistory = rtabmap::Parameters::defaultOdomBowLocalHistorySize();
bool p2p = rtabmap::Parameters::defaultOdomPnPEstimation();
for(int i=1; i<argc; ++i)
{
@@ -495,6 +498,11 @@ int main (int argc, char * argv[])
flow = true;
continue;
}
if(strcmp(argv[i], "-mono") == 0)
{
mono = true;
continue;
}
if(strcmp(argv[i], "-p2p") == 0)
{
p2p = true;
@@ -604,7 +612,6 @@ int main (int argc, char * argv[])
UINFO("Delay = %f s", sec);
UINFO("Max depth = %f", maxDepth);
UINFO("Reset odometry coutdown = %d", resetCountdown);
UINFO("Local history = %d", localHistory);
QApplication app(argc, argv);
@@ -614,20 +621,42 @@ int main (int argc, char * argv[])
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomMaxDepth(), uNumber2Str(maxDepth)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomResetCountdown(), uNumber2Str(resetCountdown)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomBowLocalHistorySize(), uNumber2Str(localHistory)));
if(!icp)
{
UINFO("Min inliers = %d", minInliers);
UINFO("Inlier maximum correspondences distance = %f", distance);
UINFO("RANSAC iterations = %d", iterations);
UINFO("Max features = %d", maxWords);
UINFO("GPU = %s", gpu?"true":"false");
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomInlierDistance(), uNumber2Str(distance)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomMinInliers(), uNumber2Str(minInliers)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomIterations(), uNumber2Str(iterations)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomMaxFeatures(), uNumber2Str(maxWords)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomFeatureType(), uNumber2Str(odomType)));
if(odomType == 0)
{
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kSURFGpuVersion(), uBool2Str(gpu)));
}
if(odomType == 2)
{
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kORBGpu(), uBool2Str(gpu)));
}
if(odomType == 3 || odomType == 4)
{
UINFO("FAST threshold = %d", fastThr);
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kFASTThreshold(), uNumber2Str(fastThr)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kFASTGpu(), uBool2Str(gpu)));
}
if(odomType == 4 || odomType == 6)
{
UINFO("BRIEF bytes = %d", briefBytes);
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kBRIEFBytes(), uNumber2Str(briefBytes)));
}
if(flow)
{
// Optical Flow
UINFO("Min inliers = %d", minInliers);
UINFO("Inlier maximum correspondences distance = %f", distance);
UINFO("RANSAC iterations = %d", iterations);
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomInlierDistance(), uNumber2Str(distance)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomMinInliers(), uNumber2Str(minInliers)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomIterations(), uNumber2Str(iterations)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomFeatureType(), uNumber2Str(odomType)));
odom = new rtabmap::OdometryOpticalFlow(parameters);
}
else
@@ -635,42 +664,25 @@ int main (int argc, char * argv[])
//BOW
UINFO("Nearest neighbor = %s", nnName.c_str());
UINFO("Nearest neighbor ratio = %f", nndr);
UINFO("Max features = %d", maxWords);
UINFO("Min inliers = %d", minInliers);
UINFO("Inlier maximum correspondences distance = %f", distance);
UINFO("RANSAC iterations = %d", iterations);
UINFO("GPU = %s", gpu?"true":"false");
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomMaxFeatures(), uNumber2Str(maxWords)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomInlierDistance(), uNumber2Str(distance)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomMinInliers(), uNumber2Str(minInliers)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomIterations(), uNumber2Str(iterations)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomFeatureType(), uNumber2Str(odomType)));
UINFO("Local history = %d", localHistory);
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomBowNNType(), uNumber2Str(nnType)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomBowNNDR(), uNumber2Str(nndr)));
if(odomType == 0)
{
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kSURFGpuVersion(), uBool2Str(gpu)));
}
if(odomType == 2)
{
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kORBGpu(), uBool2Str(gpu)));
}
if(odomType == 3 || odomType == 4)
{
UINFO("FAST threshold = %d", fastThr);
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kFASTThreshold(), uNumber2Str(fastThr)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kFASTGpu(), uBool2Str(gpu)));
}
if(odomType == 4 || odomType == 6)
{
UINFO("BRIEF bytes = %d", briefBytes);
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kBRIEFBytes(), uNumber2Str(briefBytes)));
}
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomBowLocalHistorySize(), uNumber2Str(localHistory)));
odom = new rtabmap::OdometryBOW(parameters);
if(mono)
{
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomPnPFlags(), uNumber2Str(cv::ITERATIVE)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomPnPReprojError(), "4.0"));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomIterations(), "100"));
odom = new rtabmap::OdometryMono(parameters);
}
else
{
odom = new rtabmap::OdometryBOW(parameters);
}
}
}
else // ICP
else if(icp) // ICP
{
UINFO("ICP maximum correspondences distance = %f", distance);
UINFO("ICP iterations = %d", iterations);
@@ -682,6 +694,7 @@ int main (int argc, char * argv[])
odom = new rtabmap::OdometryICP(decimation, voxel, samples, distance, iterations, ratio, !p2p);
}
rtabmap::OdometryThread odomThread(odom);
rtabmap::OdometryViewer odomViewer(maxClouds, 2, 0.0, 50);
UEventsManager::addHandler(&odomThread);