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

rtabmap:

Browse Source 
-Fixed map id not incremented on Odometry reset (identity transform was missed)
-Keeping maximum odometry variance between two loop closure updates
Odometry:
-removed parameters Odom/FeaturesRatio, Odom/LinearUpdate, Odom/AngularUpdate
-added parameter Odom/FillInfoData
-expended OdometryInfo class with features stuff
Gui:
-show inliers/outliers features in Odometry view
This commit is contained in:
Mathieu Labbe
2014-12-18 16:54:46 -05:00
parent 744e2fb3c7
commit 86a6fe202c
17 changed files with 325 additions and 382 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/Odometry.h
+2 -8
View File
@@ -60,8 +60,6 @@ public:
Transform process(const SensorData & data, OdometryInfo * info = 0);
virtual void reset(const Transform & initialPose = Transform::getIdentity());
bool isLargeEnoughTransform(const Transform & transform);
//getters
const Transform & getPose() const {return _pose;}
int getMaxFeatures() const {return _maxFeatures;}
@@ -70,10 +68,8 @@ public:
float getInlierDistance() const {return _inlierDistance;}
int getIterations() const {return _iterations;}
int getRefineIterations() const {return _refineIterations;}
float getFeaturesRatio() const {return _featuresRatio;}
float getMaxDepth() const {return _maxDepth;}
float geLinearUpdate() const {return _linearUpdate;}
float getAngularUpdate() const {return _angularUpdate;}
bool isInfoDataFilled() const {return _fillInfoData;}
private:
virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0) = 0;
@@ -85,12 +81,10 @@ private:
float _inlierDistance;
int _iterations;
int _refineIterations;
float _featuresRatio;
float _maxDepth;
float _linearUpdate;
float _angularUpdate;
int _resetCountdown;
bool _force2D;
bool _fillInfoData;
Transform _pose;
int _resetCurrentCount;
corelib/include/rtabmap/core/OdometryInfo.h
+14 -1
View File
@@ -40,7 +40,8 @@ public:
variance(-1),
features(-1),
localMapSize(-1),
time(0.0f)
time(-1),
type(-1)
{}
bool lost;
int matches;
@@ -49,6 +50,18 @@ public:
int features;
int localMapSize;
float time;
int type; // 0=BOW, 1=Optical Flow, 2=ICP
// BOW odometry
std::multimap<int, cv::KeyPoint> words;
std::vector<int> wordMatches;
std::vector<int> wordInliers;
// Optical Flow odometry
std::vector<cv::KeyPoint> refCorners;
std::vector<cv::KeyPoint> newCorners;
std::vector<int> cornerInliers;
};
}
corelib/include/rtabmap/core/Parameters.h
+1 -3
View File
@@ -296,8 +296,6 @@ class RTABMAP_EXP Parameters
// Odometry
RTABMAP_PARAM(Odom, Strategy, int, 0, "0=Bag-of-words 1=Optical Flow");
RTABMAP_PARAM(Odom, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
RTABMAP_PARAM(Odom, LinearUpdate, float, 0.0, "Min linear displacement to update odometry.");
RTABMAP_PARAM(Odom, AngularUpdate, float, 0.0, "Min angular displacement to update odometry.");
RTABMAP_PARAM(Odom, MaxFeatures, int, 0, "0 no limits.");
RTABMAP_PARAM(Odom, InlierDistance, float, 0.02, "Maximum distance for visual word correspondences.");
RTABMAP_PARAM(Odom, MinInliers, int, 20, "Minimum visual word correspondences to compute geometry transform.");
@@ -306,8 +304,8 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Odom, MaxDepth, float, 4.0, "Max depth of the words (0 means no limit).");
RTABMAP_PARAM(Odom, ResetCountdown, int, 0, "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset).");
RTABMAP_PARAM_STR(Odom, RoiRatios, "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
RTABMAP_PARAM(Odom, FeaturesRatio, float, 0.0, "Minimum ratio of keypoints between the current image and the last image to compute odometry.");
RTABMAP_PARAM(Odom, Force2D, bool, false, "Force 2D transform (3Dof: x,y and yaw).");
RTABMAP_PARAM(Odom, FillInfoData, bool, false, "Fill info with data (inliers/outliers features).");
// Odometry Bag-of-words
RTABMAP_PARAM(OdomBow, LocalHistorySize, int, 1000, "Local history size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
corelib/include/rtabmap/core/RtabmapThread.h
+2
View File
@@ -109,6 +109,8 @@ private:
Rtabmap * _rtabmap;
bool _paused;
Transform lastPose_;
float _variance;
};
} /* namespace rtabmap */
corelib/src/Odometry.cpp
+142 -174
View File
@@ -63,26 +63,22 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
_inlierDistance(Parameters::defaultOdomInlierDistance()),
_iterations(Parameters::defaultOdomIterations()),
_refineIterations(Parameters::defaultOdomRefineIterations()),
_featuresRatio(Parameters::defaultOdomFeaturesRatio()),
_maxDepth(Parameters::defaultOdomMaxDepth()),
_linearUpdate(Parameters::defaultOdomLinearUpdate()),
_angularUpdate(Parameters::defaultOdomAngularUpdate()),
_resetCountdown(Parameters::defaultOdomResetCountdown()),
_force2D(Parameters::defaultOdomForce2D()),
_fillInfoData(Parameters::defaultOdomFillInfoData()),
_resetCurrentCount(0)
{
Parameters::parse(parameters, Parameters::kOdomLinearUpdate(), _linearUpdate);
Parameters::parse(parameters, Parameters::kOdomAngularUpdate(), _angularUpdate);
Parameters::parse(parameters, Parameters::kOdomResetCountdown(), _resetCountdown);
Parameters::parse(parameters, Parameters::kOdomMinInliers(), _minInliers);
Parameters::parse(parameters, Parameters::kOdomInlierDistance(), _inlierDistance);
Parameters::parse(parameters, Parameters::kOdomIterations(), _iterations);
Parameters::parse(parameters, Parameters::kOdomRefineIterations(), _refineIterations);
Parameters::parse(parameters, Parameters::kOdomFeaturesRatio(), _featuresRatio);
Parameters::parse(parameters, Parameters::kOdomMaxDepth(), _maxDepth);
Parameters::parse(parameters, Parameters::kOdomMaxFeatures(), _maxFeatures);
Parameters::parse(parameters, Parameters::kOdomRoiRatios(), _roiRatios);
Parameters::parse(parameters, Parameters::kOdomForce2D(), _force2D);
Parameters::parse(parameters, Parameters::kOdomFillInfoData(), _fillInfoData);
}
void Odometry::reset(const Transform & initialPose)
@@ -105,27 +101,14 @@ void Odometry::reset(const Transform & initialPose)
}
}
bool Odometry::isLargeEnoughTransform(const Transform & transform)
{
float x,y,z, roll,pitch,yaw;
transform.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
return (_linearUpdate == 0.0f && _angularUpdate == 0.0f) ||
fabs(x) > _linearUpdate ||
fabs(y) > _linearUpdate ||
fabs(z) > _linearUpdate ||
fabs(roll) > _angularUpdate ||
fabs(pitch) > _angularUpdate ||
fabs(yaw) > _angularUpdate;
}
Transform Odometry::process(const SensorData & data, OdometryInfo * info)
{
UTimer time;
if(_pose.isNull())
{
_pose.setIdentity(); // initialized
}
UTimer time;
Transform t = this->computeTransform(data, info);
if(info)
@@ -145,7 +128,7 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
t = Transform(x,y,0, 0,0,yaw);
}
return _pose *= t;
return _pose *= t; // updated
}
else if(_resetCurrentCount > 0)
{
@@ -154,10 +137,11 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
--_resetCurrentCount;
if(_resetCurrentCount == 0)
{
UWARN("Odometry automatically reset!");
this->reset();
UWARN("Odometry automatically reset to latest pose!");
this->reset(_pose);
}
}
return Transform();
}
@@ -242,7 +226,12 @@ Transform OdometryBOW::computeTransform(
UTimer timer;
Transform output;
double variance = -1;
if(info)
{
info->type = 0;
}
double variance = 0;
int inliers = 0;
int correspondences = 0;
int nFeatures = 0;
@@ -254,18 +243,17 @@ Transform OdometryBOW::computeTransform(
if(newSignature)
{
nFeatures = (int)newSignature->getWords().size();
if(this->isInfoDataFilled() && info)
{
info->words = newSignature->getWords();
}
}
if(previousSignature && newSignature)
{
Transform transform;
std::set<int> uniqueCorrespondences;
if(newSignature->getWords3().size() < (unsigned int)(this->getFeaturesRatio() * float(previousSignature->getWords3().size())))
{
UWARN("At least %f%% keypoints of the last image required. New=%d last=%d",
this->getFeaturesRatio()*100.0f, newSignature->getWords3().size(), previousSignature->getWords3().size());
}
else if(!localMap_.empty() && !newSignature->getWords3().empty())
if(!localMap_.empty() && !newSignature->getWords3().empty())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr inliers1(new pcl::PointCloud<pcl::PointXYZ>); // previous
pcl::PointCloud<pcl::PointXYZ>::Ptr inliers2(new pcl::PointCloud<pcl::PointXYZ>); // new
@@ -283,6 +271,11 @@ Transform OdometryBOW::computeTransform(
UDEBUG("localMap=%d, new=%d, unique correspondences=%d", (int)localMap_.size(), (int)newSignature->getWords3().size(), (int)uniqueCorrespondences.size());
if(this->isInfoDataFilled() && info)
{
info->wordMatches.insert(info->wordMatches.end(), uniqueCorrespondences.begin(), uniqueCorrespondences.end());
}
correspondences = (int)inliers1->size();
if((int)inliers1->size() >= this->getMinInliers())
{
@@ -307,6 +300,15 @@ Transform OdometryBOW::computeTransform(
transform = this->getPose().inverse() * transform;
UDEBUG("Odom transform = %s", transform.prettyPrint().c_str());
if(this->isInfoDataFilled() && info && inliersV.size())
{
info->wordInliers.resize(inliersV.size());
for(unsigned int i=0; i<inliersV.size(); ++i)
{
info->wordInliers[i] = info->wordMatches[inliersV[i]];
}
}
}
else
{
@@ -388,54 +390,46 @@ Transform OdometryBOW::computeTransform(
else
{
output = transform;
if(!isLargeEnoughTransform(transform))
// remove words if history max size is reached
while(localMap_.size() && (int)localMap_.size() > _localHistoryMaxSize && _memory->getStMem().size()>1)
{
// Transform not large enough, keep the old signature
_memory->deleteLocation(newSignature->id());
int nodeId = *_memory->getStMem().begin();
std::list<int> removedPts;
_memory->deleteLocation(nodeId, &removedPts);
for(std::list<int>::iterator iter = removedPts.begin(); iter!=removedPts.end(); ++iter)
{
localMap_.erase(*iter);
}
}
else
if(_localHistoryMaxSize == 0 && localMap_.size() > 0 && localMap_.size() > newSignature->getWords3().size())
{
// remove words if history max size is reached
while(localMap_.size() && (int)localMap_.size() > _localHistoryMaxSize && _memory->getStMem().size()>1)
{
int nodeId = *_memory->getStMem().begin();
std::list<int> removedPts;
_memory->deleteLocation(nodeId, &removedPts);
for(std::list<int>::iterator iter = removedPts.begin(); iter!=removedPts.end(); ++iter)
{
localMap_.erase(*iter);
}
}
UERROR("Local map should have only words of the last added signature here! (size=%d, max history size=%d, newWords=%d)",
(int)localMap_.size(), _localHistoryMaxSize, (int)newSignature->getWords3().size());
}
if(_localHistoryMaxSize == 0 && localMap_.size() > 0 && localMap_.size() > newSignature->getWords3().size())
// update local map
std::list<int> uniques = uUniqueKeys(newSignature->getWords3());
Transform t = this->getPose()*output;
for(std::list<int>::iterator iter = uniques.begin(); iter!=uniques.end(); ++iter)
{
// Only add unique words not in local map
if(newSignature->getWords3().count(*iter) == 1)
{
UERROR("Local map should have only words of the last added signature here! (size=%d, max history size=%d, newWords=%d)",
(int)localMap_.size(), _localHistoryMaxSize, (int)newSignature->getWords3().size());
}
// update local map
std::list<int> uniques = uUniqueKeys(newSignature->getWords3());
Transform t = this->getPose()*output;
for(std::list<int>::iterator iter = uniques.begin(); iter!=uniques.end(); ++iter)
{
// Only add unique words not in local map
if(newSignature->getWords3().count(*iter) == 1)
// keep old word
if(localMap_.find(*iter) == localMap_.end())
{
// keep old word
if(localMap_.find(*iter) == localMap_.end())
const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second;
if(pcl::isFinite(pt))
{
const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second;
if(pcl::isFinite(pt))
{
pcl::PointXYZ pt2 = util3d::transformPoint(pt, t);
localMap_.insert(std::make_pair(*iter, pt2));
}
pcl::PointXYZ pt2 = util3d::transformPoint(pt, t);
localMap_.insert(std::make_pair(*iter, pt2));
}
}
else
{
localMap_.erase(*iter);
}
}
else
{
localMap_.erase(*iter);
}
}
}
@@ -443,28 +437,37 @@ Transform OdometryBOW::computeTransform(
else if(!previousSignature && newSignature)
{
localMap_.clear();
output.setIdentity();
int count = 0;
std::list<int> uniques = uUniqueKeys(newSignature->getWords3());
Transform t = this->getPose(); // initial pose maybe not identity...
for(std::list<int>::iterator iter = uniques.begin(); iter!=uniques.end(); ++iter)
if((int)uniques.size() >= this->getMinInliers())
{
// Only add unique words
if(newSignature->getWords3().count(*iter) == 1)
output.setIdentity();
Transform t = this->getPose(); // initial pose maybe not identity...
for(std::list<int>::iterator iter = uniques.begin(); iter!=uniques.end(); ++iter)
{
const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second;
if(pcl::isFinite(pt))
// Only add unique words
if(newSignature->getWords3().count(*iter) == 1)
{
pcl::PointXYZ pt2 = util3d::transformPoint(pt, t);
localMap_.insert(std::make_pair(*iter, pt2));
}
else
{
++count;
const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second;
if(pcl::isFinite(pt))
{
pcl::PointXYZ pt2 = util3d::transformPoint(pt, t);
localMap_.insert(std::make_pair(*iter, pt2));
}
else
{
++count;
}
}
}
}
else
{
// not enough features, just delete it
_memory->deleteLocation(newSignature->id());
}
UDEBUG("uniques=%d, pt not finite = %d", (int)uniques.size(),count);
}
@@ -480,12 +483,13 @@ Transform OdometryBOW::computeTransform(
info->localMapSize = (int)localMap_.size();
}
UINFO("Odom update time = %fs out=[%s] features=%d inliers=%d/%d local_map=%d[%d] dict=%d nodes=%d",
UINFO("Odom update time = %fs out=[%s] features=%d inliers=%d/%d variance=%f local_map=%d[%d] dict=%d nodes=%d",
timer.elapsed(),
output.prettyPrint().c_str(),
nFeatures,
inliers,
correspondences,
variance,
(int)uUniqueKeys(localMap_).size(),
(int)localMap_.size(),
(int)_memory->getVWDictionary()->getVisualWords().size(),
@@ -553,6 +557,11 @@ Transform OdometryOpticalFlow::computeTransform(
{
UDEBUG("");
if(info)
{
info->type = 1;
}
if(!data.rightImage().empty())
{
//stereo
@@ -572,7 +581,7 @@ Transform OdometryOpticalFlow::computeTransformStereo(
UTimer timer;
Transform output;
double variance = -1;
double variance = 0;
int inliers = 0;
int correspondences = 0;
@@ -642,21 +651,6 @@ Transform OdometryOpticalFlow::computeTransformStereo(
cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
UDEBUG("");
std::vector<cv::KeyPoint> lastKpts, newKpts;
/*cv::KeyPoint::convert(lastCornersKept, lastKpts);
cv::KeyPoint::convert(newCornersKept, newKpts);
std::vector<cv::DMatch> good_matches(lastKpts.size());
for(unsigned int i=0; i<good_matches.size(); ++i)
{
good_matches[i].trainIdx = i;
good_matches[i].queryIdx = i;
}
cv::drawMatches( lastFrame_, lastKpts, newLeftFrame, newKpts,
good_matches, imgMatches_, cv::Scalar::all(-1), cv::Scalar::all(-1),
std::vector<char>(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );
UDEBUG("");*/
std::vector<unsigned char> statusNew;
std::vector<float> errNew;
std::vector<cv::Point2f> newCornersKeptRight;
@@ -678,8 +672,11 @@ Transform OdometryOpticalFlow::computeTransformStereo(
correspondencesLast->resize(statusLast.size());
correspondencesNew->resize(statusLast.size());
int oi = 0;
lastKpts.resize(statusLast.size());
newKpts.resize(statusLast.size());
if(this->isInfoDataFilled() && info)
{
info->refCorners.resize(statusLast.size());
info->newCorners.resize(statusLast.size());
}
for(unsigned int i=0; i<statusLast.size(); ++i)
{
if(statusLast[i] && statusNew[i])
@@ -708,8 +705,11 @@ Transform OdometryOpticalFlow::computeTransformStereo(
newPt3D = util3d::transformPoint(newPt3D, data.localTransform());
correspondencesLast->at(oi) = lastPt3D;
correspondencesNew->at(oi) = newPt3D;
lastKpts[oi].pt = lastCornersKept[i];
newKpts[oi].pt = newCornersKept[i];
if(this->isInfoDataFilled() && info)
{
info->refCorners[oi].pt = lastCornersKept[i];
info->newCorners[oi].pt = newCornersKept[i];
}
++oi;
}
}
@@ -717,26 +717,15 @@ Transform OdometryOpticalFlow::computeTransformStereo(
}// end loop
correspondencesLast->resize(oi);
correspondencesNew->resize(oi);
lastKpts.resize(oi);
newKpts.resize(oi);
if(this->isInfoDataFilled() && info)
{
info->refCorners.resize(oi);
info->newCorners.resize(oi);
}
correspondences = oi;
refCorners3D_ = correspondencesNew;
UDEBUG("Getting correspondences end, kept %d/%d", correspondences, (int)statusLast.size());
/*good_matches.resize(lastKpts.size());
for(unsigned int i=0; i<good_matches.size(); ++i)
{
good_matches[i].trainIdx = i;
good_matches[i].queryIdx = i;
}
cv::Mat imgInliers;
cv::drawMatches( lastFrame_, lastKpts, newLeftFrame, newKpts,
good_matches, imgInliers, cv::Scalar::all(-1), cv::Scalar::all(-1),
std::vector<char>(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );
imgMatches_.push_back(imgInliers);
UDEBUG("");*/
if(correspondences >= this->getMinInliers())
{
std::vector<int> inliersV;
@@ -757,15 +746,10 @@ Transform OdometryOpticalFlow::computeTransformStereo(
output.setNull();
UWARN("Transform not valid (inliers = %d/%d)", inliers, correspondences);
}
//if(correspondencesLast->size() >= 6)
//{
// UWARN("saved pcd");
// pcl::io::savePCDFile("last.pcd", *correspondencesLast);
// pcl::io::savePCDFile("new.pcd", *correspondencesNew);
//correspondencesNew = util3d::transformPointCloud(correspondencesNew, output);
//pcl::io::savePCDFile("new2.pcd", *correspondencesNew);
//}
else if(this->isInfoDataFilled() && info && !output.isNull())
{
info->cornerInliers = inliersV;
}
}
else
{
@@ -815,12 +799,7 @@ Transform OdometryOpticalFlow::computeTransformStereo(
}
}
if(refCorners_.size() && newCorners.size() < (unsigned int)(this->getFeaturesRatio() * float(refCorners_.size())))
{
UWARN("At least %f%% keypoints of the last image required. New=%d last=%d",
this->getFeaturesRatio()*100.0f, newCorners.size(), refCorners_.size());
}
else if((int)newCorners.size() > this->getMinInliers())
if((int)newCorners.size() > this->getMinInliers())
{
refFrame_ = newLeftFrame;
refRightFrame_ = newRightFrame;
@@ -830,12 +809,9 @@ Transform OdometryOpticalFlow::computeTransformStereo(
{
UWARN("Too low 2D corners (%d), ignoring new frame...",
(int)newCorners.size());
output.setNull();
}
}
else if(!output.isNull())
{
output.setNull();
}
if(info)
{
@@ -862,7 +838,7 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
UTimer timer;
Transform output;
double variance = -1;
double variance = 0;
int inliers = 0;
int correspondences = 0;
@@ -877,9 +853,6 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
newFrame = data.image().clone();
}
float updatePixels = 0.0f;
bool updateFrame = false;
std::vector<cv::Point2f> newCorners;
if(!refFrame_.empty() && refCorners_.size() && refCorners3D_->size())
{
@@ -904,8 +877,11 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
correspondencesNew->resize(refCorners_.size());
int oi=0;
std::vector<cv::KeyPoint> lastKpts(refCorners_.size());
std::vector<cv::KeyPoint> newKpts(refCorners_.size());
if(this->isInfoDataFilled() && info)
{
info->refCorners.resize(refCorners_.size());
info->newCorners.resize(refCorners_.size());
}
UASSERT(refCorners_.size() == refCorners3D_->size());
UDEBUG("lastCorners3D_ = %d", refCorners3D_->size());
@@ -932,8 +908,11 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
cv::Point2f diff = newCorners[i]-refCorners_[i];
sumSqrdDistance += diff.x*diff.x + diff.y*diff.y;
lastKpts[oi].pt = refCorners_[i];
newKpts[oi].pt = newCorners[i];
if(this->isInfoDataFilled() && info)
{
info->refCorners[oi].pt = refCorners_[i];
info->newCorners[oi].pt = newCorners[i];
}
++oi;
}
@@ -945,19 +924,12 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
}
}
UDEBUG("Flow inliers = %d, added inliers=%d", flowInliers, oi);
float meanPixel = -1;
if(oi)
{
float meanPixel = sumSqrdDistance/(float)oi;
if(meanPixel >= updatePixels*updatePixels)
{
updateFrame = true;
}
if(this->isInfoDataFilled() && info)
{
info->refCorners.resize(oi);
info->newCorners.resize(oi);
}
UDEBUG("mean pixel distance = %f", meanPixel);
lastKpts.resize(oi);
newKpts.resize(oi);
correspondencesLast->resize(oi);
correspondencesNew->resize(oi);
correspondences = oi;
@@ -981,6 +953,10 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
output.setNull();
UWARN("Transform not valid (inliers = %d/%d)", inliers, correspondences);
}
else if(this->isInfoDataFilled() && info && !output.isNull())
{
info->cornerInliers = inliersV;
}
/*std::vector<cv::DMatch> good_matches(lastKpts.size());
for(unsigned int i=0; i<good_matches.size(); ++i)
@@ -1002,11 +978,10 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
{
//return Identity
output = Transform::getIdentity();
updateFrame = true;
}
newCorners.clear();
if(!output.isNull() && updateFrame)
if(!output.isNull())
{
// Copy or generate new keypoints
if(data.keypoints().size())
@@ -1040,12 +1015,7 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
}
}
if(refCorners_.size() && newCorners.size() < (unsigned int)(this->getFeaturesRatio() * float(refCorners_.size())))
{
UWARN("At least %f%% keypoints of the last image required. New=%d last=%d",
this->getFeaturesRatio()*100.0f, newCorners.size(), refCorners_.size());
}
else if((int)newCorners.size() > this->getMinInliers())
if((int)newCorners.size() > this->getMinInliers())
{
// get 3D corners for the extracted 2D corners (not the ones refined by Optical Flow)
pcl::PointCloud<pcl::PointXYZ>::Ptr newCorners3D(new pcl::PointCloud<pcl::PointXYZ>);
@@ -1084,18 +1054,16 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
{
UWARN("Too low 3D corners (%d/%d, minCorners=%d), ignoring new frame...",
(int)newCornersFiltered.size(), (int)refCorners3D_->size(), this->getMinInliers());
output.setNull();
}
}
else
{
UWARN("Too low 2D corners (%d), ignoring new frame...",
(int)newCorners.size());
output.setNull();
}
}
else if(!output.isNull())
{
output = Transform::getIdentity();
}
if(info)
{
@@ -1105,12 +1073,12 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
info->matches = correspondences;
}
UINFO("Odom update time = %fs inliers=%d/%d, new corners=%d, transform accepted=%s",
UINFO("Odom update time = %fs inliers=%d/%d, variance=%f, new corners=%d",
timer.elapsed(),
inliers,
correspondences,
(int)newCorners.size(),
updateFrame||output.isNull()?"true":"false");
variance,
(int)newCorners.size());
return output;
}
@@ -1150,7 +1118,7 @@ Transform OdometryICP::computeTransform(const SensorData & data, OdometryInfo *
Transform output;
bool hasConverged = false;
double variance = -1;
double variance = 0;
unsigned int minPoints = 100;
if(!data.depth().empty())
{
corelib/src/Rtabmap.cpp
+9 -19
View File
@@ -771,30 +771,20 @@ bool Rtabmap::process(const SensorData & data)
if(_memory->getLastWorkingSignature())
{
const Transform & lastPose = _memory->getLastWorkingSignature()->getPose(); // use raw odometry
if(!lastPose.isIdentity() && data.pose().isIdentity())
Transform lastPoseToNewPose = lastPose.inverse() * data.pose();
float x,y,z, roll,pitch,yaw;
lastPoseToNewPose.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
if(_newMapOdomChangeDistance > 0.0 && (x*x + y*y + z*z) > _newMapOdomChangeDistance*_newMapOdomChangeDistance)
{
int mapId = _memory->incrementMapId();
UWARN("Odometry is reset (transform identity detected). A new map (%d) is created!", mapId);
UWARN("Odometry is reset (large odometry change detected > %f). A new map (%d) is created! Last pose = %s, new pose = %s",
_newMapOdomChangeDistance,
mapId,
lastPose.prettyPrint().c_str(),
data.pose().prettyPrint().c_str());
_optimizedPoses.clear();
_constraints.clear();
}
else
{
Transform lastPoseToNewPose = lastPose.inverse() * data.pose();
float x,y,z, roll,pitch,yaw;
lastPoseToNewPose.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
if(_newMapOdomChangeDistance > 0.0 && (x*x + y*y + z*z) > _newMapOdomChangeDistance*_newMapOdomChangeDistance)
{
int mapId = _memory->incrementMapId();
UWARN("Odometry is reset (large odometry change detected > %f). A new map (%d) is created! Last pose = %s, new pose = %s",
_newMapOdomChangeDistance,
mapId,
lastPose.prettyPrint().c_str(),
data.pose().prettyPrint().c_str());
_optimizedPoses.clear();
_constraints.clear();
}
}
}
}
}
corelib/src/RtabmapThread.cpp
+29 -2
View File
@@ -46,7 +46,9 @@ RtabmapThread::RtabmapThread(Rtabmap * rtabmap) :
_rate(Parameters::defaultRtabmapDetectionRate()),
_frameRateTimer(new UTimer()),
_rtabmap(rtabmap),
_paused(false)
_paused(false),
lastPose_(Transform::getIdentity()),
_variance(0)
{
UASSERT(rtabmap != 0);
@@ -82,6 +84,8 @@ void RtabmapThread::clearBufferedData()
_dataMutex.lock();
{
_dataBuffer.clear();
lastPose_.setIdentity();
_variance = 0;
}
_dataMutex.unlock();
}
@@ -253,6 +257,10 @@ void RtabmapThread::handleEvent(UEvent* event)
{
this->addData(e->data());
}
else
{
lastPose_.setNull();
}
}
else if(event->getClassName().compare("RtabmapEventCmd") == 0)
{
@@ -392,7 +400,7 @@ void RtabmapThread::process()
{
SensorData data;
getData(data);
if(data.isValid())
if(data.isValid() && _state.empty())
{
if(_rtabmap->getMemory())
{
@@ -421,6 +429,19 @@ void RtabmapThread::addData(const SensorData & sensorData)
return;
}
if(!lastPose_.isIdentity() && sensorData.pose().isIdentity())
{
UWARN("Odometry is reset (identity pose detected). Increment map id!");
pushNewState(kStateTriggeringMap);
_variance = 0;
}
lastPose_ = sensorData.pose();
if(sensorData.poseVariance() > _variance)
{
_variance = sensorData.poseVariance();
}
if(_rate>0.0f)
{
if(_frameRateTimer->getElapsedTime() < 1.0f/_rate)
@@ -434,6 +455,12 @@ void RtabmapThread::addData(const SensorData & sensorData)
_dataMutex.lock();
{
_dataBuffer.push_back(sensorData);
if(_variance <= 0)
{
_variance = 1.0f;
}
_dataBuffer.back().setPose(_dataBuffer.back().pose(), _variance);
_variance = 0;
while(_dataBufferMaxSize > 0 && _dataBuffer.size() > (unsigned int)_dataBufferMaxSize)
{
ULOGGER_WARN("Data buffer is full, the oldest data is removed to add the new one.");
corelib/src/util3d.cpp
+1 -1
View File
@@ -1504,7 +1504,7 @@ Transform transformFromXYZCorrespondences(
if(varianceOut)
{
*varianceOut = 1.0f;
*varianceOut = 1.0;
}
Transform transform;
if(cloud1->size() >=3 && cloud1->size() == cloud2->size())
guilib/include/rtabmap/gui/ImageView.h
+2 -1
View File
@@ -63,7 +63,8 @@ public:
void setImageDepthShown(bool shown);
void setLinesShown(bool shown);
void setFeatures(const std::multimap<int, cv::KeyPoint> & refWords);
void setFeatures(const std::multimap<int, cv::KeyPoint> & refWords, const QColor & color = QColor(255, 255, 0, 70));
void setFeatures(const std::vector<cv::KeyPoint> & features, const QColor & color = QColor(255, 255, 0, 70));
void setImage(const QImage & image);
void setImageDepth(const QImage & image);
void setFeatureColor(int id, const QColor & color);
guilib/include/rtabmap/gui/KeypointItem.h
+2
View File
@@ -43,6 +43,8 @@ public:
KeypointItem(qreal x, qreal y, int r, const QString & info, const QColor & color = Qt::green, QGraphicsItem * parent = 0);
virtual ~KeypointItem();
void setColor(const QColor & color);
protected:
virtual void hoverEnterEvent ( QGraphicsSceneHoverEvent * event );
virtual void hoverLeaveEvent ( QGraphicsSceneHoverEvent * event );
guilib/src/ImageView.cpp
+38 -6
View File
@@ -201,6 +201,14 @@ void ImageView::updateOpacity()
_imageDepth->setGraphicsEffect(0);
}
}
else if(_image)
{
_image->setGraphicsEffect(0);
}
else if(_imageDepth)
{
_imageDepth->setGraphicsEffect(0);
}
}
void ImageView::updateZoom()
@@ -242,13 +250,12 @@ void ImageView::wheelEvent(QWheelEvent * e)
this->setMatrix(matrix);
}
void ImageView::setFeatures(const std::multimap<int, cv::KeyPoint> & refWords)
void ImageView::setFeatures(const std::multimap<int, cv::KeyPoint> & refWords, const QColor & color)
{
qDeleteAll(_features);
_features.clear();
rtabmap::KeypointItem * item = 0;
int alpha = 70;
for(std::multimap<int, cv::KeyPoint>::const_iterator i = refWords.begin(); i != refWords.end(); ++i )
{
const cv::KeyPoint & r = (*i).second;
@@ -260,9 +267,35 @@ void ImageView::setFeatures(const std::multimap<int, cv::KeyPoint> & refWords)
"X = %5\n"
"Y = %6\n"
"Size = %7").arg(id).arg(1).arg(r.angle).arg(r.response).arg(r.pt.x).arg(r.pt.y).arg(r.size);
float radius = r.size*1.2/9.*2;
float radius = r.size/2.0f;
item = new rtabmap::KeypointItem(r.pt.x-radius, r.pt.y-radius, radius*2, info, color);
item = new rtabmap::KeypointItem(r.pt.x-radius, r.pt.y-radius, radius*2, info, QColor(255, 255, 0, alpha));
scene()->addItem(item);
_features.insert(id, item);
item->setVisible(_showFeatures->isChecked());
item->setZValue(1);
}
}
void ImageView::setFeatures(const std::vector<cv::KeyPoint> & features, const QColor & color)
{
qDeleteAll(_features);
_features.clear();
rtabmap::KeypointItem * item = 0;
for(unsigned int i = 0; i< features.size(); ++i )
{
const cv::KeyPoint & r = features[i];
int id = i;
QString info = QString( "WordRef = %1\n"
"Laplacian = %2\n"
"Dir = %3\n"
"Hessian = %4\n"
"X = %5\n"
"Y = %6\n"
"Size = %7").arg(id).arg(1).arg(r.angle).arg(r.response).arg(r.pt.x).arg(r.pt.y).arg(r.size);
float radius = (r.size==0?3:r.size)/2.0f;
item = new rtabmap::KeypointItem(r.pt.x-radius, r.pt.y-radius, radius*2, info, color);
scene()->addItem(item);
_features.insert(id, item);
@@ -308,8 +341,7 @@ void ImageView::setFeatureColor(int id, const QColor & color)
{
for(int i=0; i<items.size(); ++i)
{
items[i]->setPen(QPen(color));
items[i]->setBrush(QBrush(color));
items[i]->setColor(color);
}
}
else
guilib/src/KeypointItem.cpp
+9 -4
View File
@@ -39,8 +39,7 @@ KeypointItem::KeypointItem(qreal x, qreal y, int r, const QString & info, const
_info(info),
_placeHolder(0)
{
this->setPen(QPen(color));
this->setBrush(QBrush(color));
this->setColor(color);
this->setAcceptsHoverEvents(true);
this->setFlag(QGraphicsItem::ItemIsFocusable, true);
_width = pen().width();
@@ -48,10 +47,16 @@ KeypointItem::KeypointItem(qreal x, qreal y, int r, const QString & info, const
KeypointItem::~KeypointItem()
{
/*if(_placeHolder)
if(_placeHolder)
{
delete _placeHolder;
}*/
}
}
void KeypointItem::setColor(const QColor & color)
{
this->setPen(QPen(color));
this->setBrush(QBrush(color));
}
void KeypointItem::showDescription()
guilib/src/MainWindow.cpp
+46 -6
View File
@@ -566,9 +566,7 @@ void MainWindow::handleEvent(UEvent* anEvent)
else if(anEvent->getClassName().compare("OdometryEvent") == 0)
{
OdometryEvent * odomEvent = (OdometryEvent*)anEvent;
if((_ui->dockWidget_cloudViewer->isVisible() || _ui->dockWidget_odometry->isVisible()) &&
_lastOdometryProcessed &&
!_processingStatistics)
if(_lastOdometryProcessed && !_processingStatistics)
{
_lastOdometryProcessed = false; // if we receive too many odometry events!
emit odometryReceived(odomEvent->data(), odomEvent->info());
@@ -596,6 +594,7 @@ void MainWindow::handleEvent(UEvent* anEvent)
void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap::OdometryInfo & info)
{
UTimer time;
Transform pose = data.pose();
bool lost = false;
_ui->imageView_odometry->resetTransform();
@@ -725,6 +724,16 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
if(_ui->dockWidget_odometry->isVisible() &&
!data.image().empty())
{
int alpha = _preferencesDialog->getKeypointsOpacity()*255/100;
if(info.type == 0)
{
_ui->imageView_odometry->setFeatures(info.words, QColor(255,255,0, alpha));
}
else if(info.type == 1)
{
_ui->imageView_odometry->setFeatures(info.refCorners, QColor(255,0,0, alpha));
}
_ui->imageView_odometry->clearLines();
if(lost)
{
_ui->imageView_odometry->setImageDepth(uCvMat2QImage(data.image()));
@@ -736,6 +745,35 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
_ui->imageView_odometry->setImage(uCvMat2QImage(data.image()));
_ui->imageView_odometry->setImageShown(true);
_ui->imageView_odometry->setImageDepthShown(false);
if(info.type == 0)
{
for(unsigned int i=0; i<info.wordMatches.size(); ++i)
{
_ui->imageView_odometry->setFeatureColor(info.wordMatches[i], QColor(255,0,0, alpha)); // outliers
}
for(unsigned int i=0; i<info.wordInliers.size(); ++i)
{
_ui->imageView_odometry->setFeatureColor(info.wordInliers[i], QColor(0,255,0, alpha)); // inliers
}
}
else if(info.type == 1)
{
//draw lines
UASSERT(info.refCorners.size() == info.newCorners.size());
for(unsigned int i=0; i<info.cornerInliers.size(); ++i)
{
_ui->imageView_odometry->setFeatureColor(info.cornerInliers[i], QColor(0,255,0, alpha)); // inliers
QGraphicsLineItem * item = _ui->imageView_odometry->scene()->addLine(
info.refCorners[info.cornerInliers[i]].pt.x,
info.refCorners[info.cornerInliers[i]].pt.y,
info.newCorners[info.cornerInliers[i]].pt.x,
info.newCorners[info.cornerInliers[i]].pt.y,
QPen(QColor(0, 0, 255, alpha)));
item->setVisible(_ui->imageView_odometry->isLinesShown());
item->setZValue(1);
}
}
}
_ui->imageView_odometry->resetZoom();
@@ -753,6 +791,8 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
{
this->captureScreen();
}
_ui->statsToolBox->updateStat("/Gui refresh odom/ms", (float)data.id(), time.elapsed()*1000.0);
}
void MainWindow::processStats(const rtabmap::Statistics & stat)
@@ -1815,7 +1855,7 @@ void MainWindow::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords
"X = %5\n"
"Y = %6\n"
"Size = %7").arg(id).arg(1).arg(r.angle).arg(r.response).arg(r.pt.x).arg(r.pt.y).arg(r.size);
float radius = r.size*1.2/9.*2;
float radius = r.size/2.0f;
if(uContains(loopWords, id))
{
// PINK = FOUND IN LOOP SIGNATURE
@@ -1864,7 +1904,7 @@ void MainWindow::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords
"X = %5\n"
"Y = %6\n"
"Size = %7").arg(id).arg(1).arg(r.angle).arg(r.response).arg(r.pt.x).arg(r.pt.y).arg(r.size);
float radius = r.size*1.2/9.*2;
float radius = r.size/2.0f;
if(uContains(refWords, id))
{
// PINK = FOUND IN LOOP SIGNATURE
@@ -1945,7 +1985,7 @@ void MainWindow::resizeEvent(QResizeEvent* anEvent)
{
_ui->imageView_source->fitInView(_ui->imageView_source->sceneRect(), Qt::KeepAspectRatio);
_ui->imageView_loopClosure->fitInView(_ui->imageView_source->sceneRect(), Qt::KeepAspectRatio);
_ui->imageView_source->fitInView(_ui->imageView_odometry->sceneRect(), Qt::KeepAspectRatio);
_ui->imageView_odometry->fitInView(_ui->imageView_odometry->sceneRect(), Qt::KeepAspectRatio);
_ui->imageView_source->resetZoom();
_ui->imageView_loopClosure->resetZoom();
_ui->imageView_odometry->resetZoom();
guilib/src/PreferencesDialog.cpp
+1 -3
View File
@@ -482,17 +482,15 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
//Odometry
_ui->odom_strategy->setObjectName(Parameters::kOdomStrategy().c_str());
_ui->odom_type->setObjectName(Parameters::kOdomFeatureType().c_str());
_ui->odom_linearUpdate->setObjectName(Parameters::kOdomLinearUpdate().c_str());
_ui->odom_angularUpdate->setObjectName(Parameters::kOdomAngularUpdate().c_str());
_ui->odom_countdown->setObjectName(Parameters::kOdomResetCountdown().c_str());
_ui->odom_maxFeatures->setObjectName(Parameters::kOdomMaxFeatures().c_str());
_ui->odom_ratio->setObjectName(Parameters::kOdomFeaturesRatio().c_str());
_ui->odom_inlierDistance->setObjectName(Parameters::kOdomInlierDistance().c_str());
_ui->odom_iterations->setObjectName(Parameters::kOdomIterations().c_str());
_ui->odom_maxDepth->setObjectName(Parameters::kOdomMaxDepth().c_str());
_ui->odom_minInliers->setObjectName(Parameters::kOdomMinInliers().c_str());
_ui->odom_refine_iterations->setObjectName(Parameters::kOdomRefineIterations().c_str());
_ui->odom_force2D->setObjectName(Parameters::kOdomForce2D().c_str());
_ui->odom_fillInfoData->setObjectName(Parameters::kOdomFillInfoData().c_str());
_ui->lineEdit_odom_roi->setObjectName(Parameters::kOdomRoiRatios().c_str());
//Odometry BOW
guilib/src/ui/DatabaseViewer.ui
+3
View File
@@ -792,6 +792,9 @@
<property name="text">
<string/>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="0">
guilib/src/ui/preferencesDialog.ui
+24 -91
View File
@@ -63,9 +63,9 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-305</y>
<y>0</y>
<width>744</width>
<height>1252</height>
<height>1056</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>19</number>
<number>23</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29">
@@ -6261,7 +6261,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</item>
<item>
<layout class="QGridLayout" name="gridLayout_27" columnstretch="0,0">
<item row="4" column="1">
<item row="2" column="1">
<widget class="QLabel" name="label_47">
<property name="text">
<string>Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset).</string>
@@ -6288,55 +6288,13 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</item>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_46">
<property name="text">
<string>Angular update: minimum angular distance to update the odometry.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="odom_angularUpdate">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>3</number>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>0.015000000000000</double>
</property>
</widget>
</item>
<item row="4" column="0">
<item row="2" column="0">
<widget class="QSpinBox" name="odom_countdown">
<property name="maximum">
<number>999999</number>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QDoubleSpinBox" name="odom_linearUpdate">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>3</number>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>0.015000000000000</double>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QComboBox" name="odom_type">
<property name="sizeAdjustPolicy">
@@ -6384,16 +6342,6 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</item>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_45">
<property name="text">
<string>Linear update: minimum linear distance to update the odometry.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_44">
<property name="text">
@@ -6416,14 +6364,14 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property>
</widget>
</item>
<item row="5" column="0">
<item row="3" column="0">
<widget class="QCheckBox" name="odom_force2D">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="5" column="1">
<item row="3" column="1">
<widget class="QLabel" name="label_196">
<property name="text">
<string>Force 2D transform (3DoF: x,y and yaw).</string>
@@ -6433,6 +6381,23 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_221">
<property name="text">
<string>Fill info with data (inliers/outliers features to be shown in Odometry view).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QCheckBox" name="odom_fillInfoData">
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</item>
<item>
@@ -6545,38 +6510,6 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QDoubleSpinBox" name="odom_ratio">
<property name="suffix">
<string/>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.500000000000000</double>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_90">
<property name="text">
<string>Minimum ratio of keypoints between the current image and the last image to compute odometry.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</widget>
</item>
tools/OdometryViewer/main.cpp
-63
View File
@@ -60,9 +60,6 @@ void showUsage()
" -min # Minimum inliers to accept the transform (default 20)\n"
" -depth #.# Maximum features depth (default 5.0 m)\n"
" -i # RANSAC/ICP iterations (default 30)\n"
" -r #.# Words ratio (default 0.5)\n"
" -lu # Linear update (default 0.0 m)\n"
" -au # Angular update (default 0.0 radian)\n"
" -reset # Reset countdown (default 0 = disabled)\n"
" -gpu Use GPU\n"
" -lh # Local history (default 1000)\n"
@@ -105,11 +102,8 @@ int main (int argc, char * argv[])
float distance = 0.01;
int maxWords = 0;
int minInliers = 20;
float wordsRatio = 0.5;
float maxDepth = 5.0f;
int iterations = 30;
float linearUpdate = 0.0f;
float angularUpdate = 0.0f;
int resetCountdown = 0;
int decimation = 4;
float voxel = 0.005;
@@ -347,57 +341,6 @@ int main (int argc, char * argv[])
}
continue;
}
if(strcmp(argv[i], "-r") == 0)
{
++i;
if(i < argc)
{
wordsRatio = std::atof(argv[i]);
if(wordsRatio < 0.0f || wordsRatio > 1.0f)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-lu") == 0)
{
++i;
if(i < argc)
{
linearUpdate = std::atof(argv[i]);
if(linearUpdate < 0.0f)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-au") == 0)
{
++i;
if(i < argc)
{
angularUpdate = std::atof(argv[i]);
if(angularUpdate < 0.0f)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-reset") == 0)
{
++i;
@@ -655,8 +598,6 @@ int main (int argc, char * argv[])
UINFO("Maximum clouds shown = %d", maxClouds);
UINFO("Delay = %f s", sec);
UINFO("Max depth = %f", maxDepth);
UINFO("Linear update = %f", linearUpdate);
UINFO("Angular update = %f", angularUpdate);
UINFO("Reset odometry coutdown = %d", resetCountdown);
UINFO("Local history = %d", localHistory);
@@ -667,8 +608,6 @@ int main (int argc, char * argv[])
rtabmap::ParametersMap parameters;
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomMaxDepth(), uNumber2Str(maxDepth)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomLinearUpdate(), uNumber2Str(linearUpdate)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomAngularUpdate(), uNumber2Str(angularUpdate)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomResetCountdown(), uNumber2Str(resetCountdown)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomBowLocalHistorySize(), uNumber2Str(localHistory)));
@@ -693,7 +632,6 @@ int main (int argc, char * argv[])
UINFO("Nearest neighbor ratio = %f", nndr);
UINFO("Max features = %d", maxWords);
UINFO("Min inliers = %d", minInliers);
UINFO("Words ratio = %f", wordsRatio);
UINFO("Inlier maximum correspondences distance = %f", distance);
UINFO("RANSAC iterations = %d", iterations);
UINFO("GPU = %s", gpu?"true":"false");
@@ -702,7 +640,6 @@ int main (int argc, char * argv[])
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)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomFeaturesRatio(), uNumber2Str(wordsRatio)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomBowNNType(), uNumber2Str(nnType)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomBowNNDR(), uNumber2Str(nndr)));
if(odomType == 0)