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

Added constraints editing to DatabaseViewer to add/reject loop closures, refine loop closures with ICP, detect more loop closures using the optimized map

Browse Source 
Added Feature2D::parseParameters()
 

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1632 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-08-11 15:23:06 +00:00
parent 52af62394e
commit df76e3cd29
13 changed files with 1961 additions and 378 deletions
Download Patch File Download Diff File Expand all files Collapse all files

53
corelib/include/rtabmap/core/Features2d.h
View File

@@ -74,6 +74,8 @@ public:
std::vector<cv::KeyPoint> generateKeypoints(const cv::Mat & image, int maxKeypoints=0, const cv::Rect & roi = cv::Rect()) const;
cv::Mat generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
virtual void parseParameters(const ParametersMap & parameters) {}
protected:
Feature2D(const ParametersMap & parameters = ParametersMap()) {}
@@ -89,11 +91,21 @@ public:
SURF(const ParametersMap & parameters = ParametersMap());
virtual ~SURF();
virtual void parseParameters(const ParametersMap & parameters);
private:
virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi) const;
virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
private:
double hessianThreshold_;
int nOctaves_;
int nOctaveLayers_;
bool extended_;
bool upright_;
float gpuKeypointsRatio_;
bool gpuVersion_;
cv::SURF * _surf;
cv::gpu::SURF_GPU * _gpuSurf;
};
@@ -105,11 +117,19 @@ public:
SIFT(const ParametersMap & parameters = ParametersMap());
virtual ~SIFT();
virtual void parseParameters(const ParametersMap & parameters);
private:
virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi) const;
virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
private:
int nfeatures_;
int nOctaveLayers_;
double contrastThreshold_;
double edgeThreshold_;
double sigma_;
cv::SIFT * _sift;
};
@@ -120,11 +140,26 @@ public:
ORB(const ParametersMap & parameters = ParametersMap());
virtual ~ORB();
virtual void parseParameters(const ParametersMap & parameters);
private:
virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi) const;
virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
private:
int nFeatures_;
float scaleFactor_;
int nLevels_;
int edgeThreshold_;
int firstLevel_;
int WTA_K_;
int scoreType_;
int patchSize_;
bool gpu_;
int fastThreshold_;
bool nonmaxSuppresion_;
cv::ORB * _orb;
cv::gpu::ORB_GPU * _gpuOrb;
};
@@ -136,10 +171,17 @@ public:
FAST(const ParametersMap & parameters = ParametersMap());
virtual ~FAST();
virtual void parseParameters(const ParametersMap & parameters);
private:
virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi) const;
private:
int threshold_;
bool nonmaxSuppression_;
bool gpu_;
double gpuKeypointsRatio_;
cv::FastFeatureDetector * _fast;
cv::gpu::FAST_GPU * _gpuFast;
};
@@ -151,10 +193,14 @@ public:
FAST_BRIEF(const ParametersMap & parameters = ParametersMap());
virtual ~FAST_BRIEF();
virtual void parseParameters(const ParametersMap & parameters);
private:
virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
private:
int bytes_;
cv::BriefDescriptorExtractor * _brief;
};
@@ -165,10 +211,17 @@ public:
FAST_FREAK(const ParametersMap & parameters = ParametersMap());
virtual ~FAST_FREAK();
virtual void parseParameters(const ParametersMap & parameters);
private:
virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
private:
bool orientationNormalized_;
bool scaleNormalized_;
float patternScale_;
int nOctaves_;
cv::FREAK * _freak;
};

10
corelib/include/rtabmap/core/Link.h
View File

@@ -15,7 +15,13 @@ namespace rtabmap {
class Link
{
public:
enum Type {kNeighbor, kGlobalClosure, kLocalSpaceClosure, kLocalTimeClosure};
enum Type {kNeighbor, kGlobalClosure, kLocalSpaceClosure, kLocalTimeClosure, kUserClosure, kUndef};
Link() :
from_(0),
to_(0),
type_(kUndef)
{
}
Link(int from, int to, const Transform & transform, Type type) :
from_(from),
to_(to),
@@ -24,6 +30,8 @@ public:
{
}
bool isValid() const {return from_ > 0 && to_ > 0 && !transform_.isNull() && type_!=kUndef;}
int from() const {return from_;}
int to() const {return to_;}
const Transform & transform() const {return transform_;}

5
corelib/include/rtabmap/core/util3d.h
View File

@@ -390,6 +390,11 @@ std::map<int, Transform> RTABMAP_EXP radiusPosesFiltering(
float angle,
bool keepLatest = true);
std::multimap<int, int> RTABMAP_EXP radiusPosesClustering(
const std::map<int, Transform> & poses,
float radius,
float angle);
cv::Mat RTABMAP_EXP create2DMap(const std::map<int, Transform> & poses,
const std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > & scans,
float cellSize,

312
corelib/src/Features2d.cpp
View File

@@ -246,38 +246,17 @@ cv::Mat Feature2D::generateDescriptors(const cv::Mat & image, std::vector<cv::Ke
//SURF
//////////////////////////
SURF::SURF(const ParametersMap & parameters) :
hessianThreshold_(Parameters::defaultSURFHessianThreshold()),
nOctaves_(Parameters::defaultSURFOctaves()),
nOctaveLayers_(Parameters::defaultSURFOctaveLayers()),
extended_(Parameters::defaultSURFExtended()),
upright_(Parameters::defaultSURFUpright()),
gpuKeypointsRatio_(Parameters::defaultSURFGpuKeypointsRatio()),
gpuVersion_(Parameters::defaultSURFGpuVersion()),
_surf(0),
_gpuSurf(0)
{
double hessianThreshold = Parameters::defaultSURFHessianThreshold();
int nOctaves = Parameters::defaultSURFOctaves();
int nOctaveLayers = Parameters::defaultSURFOctaveLayers();
bool extended = Parameters::defaultSURFExtended();
bool upright = Parameters::defaultSURFUpright();
float gpuKeypointsRatio = Parameters::defaultSURFGpuKeypointsRatio();
bool gpuVersion = Parameters::defaultSURFGpuVersion();
Parameters::parse(parameters, Parameters::kSURFExtended(), extended);
Parameters::parse(parameters, Parameters::kSURFHessianThreshold(), hessianThreshold);
Parameters::parse(parameters, Parameters::kSURFOctaveLayers(), nOctaveLayers);
Parameters::parse(parameters, Parameters::kSURFOctaves(), nOctaves);
Parameters::parse(parameters, Parameters::kSURFUpright(), upright);
Parameters::parse(parameters, Parameters::kSURFGpuKeypointsRatio(), gpuKeypointsRatio);
Parameters::parse(parameters, Parameters::kSURFGpuVersion(), gpuVersion);
if(gpuVersion && cv::gpu::getCudaEnabledDeviceCount())
{
_gpuSurf = new cv::gpu::SURF_GPU(hessianThreshold, nOctaves, nOctaveLayers, extended, gpuKeypointsRatio, upright);
}
else
{
if(gpuVersion)
{
UWARN("GPU version of SURF not available! Using CPU version instead...");
}
_surf = new cv::SURF (hessianThreshold, nOctaves, nOctaveLayers, extended, upright);
}
parseParameters(parameters);
}
SURF::~SURF()
@@ -292,6 +271,42 @@ SURF::~SURF()
}
}
void SURF::parseParameters(const ParametersMap & parameters)
{
Parameters::parse(parameters, Parameters::kSURFExtended(), extended_);
Parameters::parse(parameters, Parameters::kSURFHessianThreshold(), hessianThreshold_);
Parameters::parse(parameters, Parameters::kSURFOctaveLayers(), nOctaveLayers_);
Parameters::parse(parameters, Parameters::kSURFOctaves(), nOctaves_);
Parameters::parse(parameters, Parameters::kSURFUpright(), upright_);
Parameters::parse(parameters, Parameters::kSURFGpuKeypointsRatio(), gpuKeypointsRatio_);
Parameters::parse(parameters, Parameters::kSURFGpuVersion(), gpuVersion_);
if(_gpuSurf)
{
delete _gpuSurf;
_gpuSurf = 0;
}
if(_surf)
{
delete _surf;
_surf = 0;
}
if(gpuVersion_ && cv::gpu::getCudaEnabledDeviceCount())
{
_gpuSurf = new cv::gpu::SURF_GPU(hessianThreshold_, nOctaves_, nOctaveLayers_, extended_, gpuKeypointsRatio_, upright_);
}
else
{
if(gpuVersion_)
{
UWARN("GPU version of SURF not available! Using CPU version instead...");
}
_surf = new cv::SURF(hessianThreshold_, nOctaves_, nOctaveLayers_, extended_, upright_);
}
}
std::vector<cv::KeyPoint> SURF::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi) const
{
UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
@@ -342,21 +357,14 @@ cv::Mat SURF::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::Key
//SIFT
//////////////////////////
SIFT::SIFT(const ParametersMap & parameters) :
nfeatures_(Parameters::defaultSIFTNFeatures()),
nOctaveLayers_(Parameters::defaultSIFTNOctaveLayers()),
contrastThreshold_(Parameters::defaultSIFTContrastThreshold()),
edgeThreshold_(Parameters::defaultSIFTEdgeThreshold()),
sigma_(Parameters::defaultSIFTSigma()),
_sift(0)
{
int nfeatures = Parameters::defaultSIFTNFeatures();
int nOctaveLayers = Parameters::defaultSIFTNOctaveLayers();
double contrastThreshold = Parameters::defaultSIFTContrastThreshold();
double edgeThreshold = Parameters::defaultSIFTEdgeThreshold();
double sigma = Parameters::defaultSIFTSigma();
Parameters::parse(parameters, Parameters::kSIFTContrastThreshold(), contrastThreshold);
Parameters::parse(parameters, Parameters::kSIFTEdgeThreshold(), edgeThreshold);
Parameters::parse(parameters, Parameters::kSIFTNFeatures(), nfeatures);
Parameters::parse(parameters, Parameters::kSIFTNOctaveLayers(), nOctaveLayers);
Parameters::parse(parameters, Parameters::kSIFTSigma(), sigma);
_sift = new cv::SIFT(nfeatures, nOctaveLayers, contrastThreshold, edgeThreshold, sigma);
parseParameters(parameters);
}
SIFT::~SIFT()
@@ -367,6 +375,23 @@ SIFT::~SIFT()
}
}
void SIFT::parseParameters(const ParametersMap & parameters)
{
Parameters::parse(parameters, Parameters::kSIFTContrastThreshold(), contrastThreshold_);
Parameters::parse(parameters, Parameters::kSIFTEdgeThreshold(), edgeThreshold_);
Parameters::parse(parameters, Parameters::kSIFTNFeatures(), nfeatures_);
Parameters::parse(parameters, Parameters::kSIFTNOctaveLayers(), nOctaveLayers_);
Parameters::parse(parameters, Parameters::kSIFTSigma(), sigma_);
if(_sift)
{
delete _sift;
_sift = 0;
}
_sift = new cv::SIFT(nfeatures_, nOctaveLayers_, contrastThreshold_, edgeThreshold_, sigma_);
}
std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi) const
{
UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
@@ -388,48 +413,21 @@ cv::Mat SIFT::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::Key
//ORB
//////////////////////////
ORB::ORB(const ParametersMap & parameters) :
nFeatures_(Parameters::defaultORBNFeatures()),
scaleFactor_(Parameters::defaultORBScaleFactor()),
nLevels_(Parameters::defaultORBNLevels()),
edgeThreshold_(Parameters::defaultORBEdgeThreshold()),
firstLevel_(Parameters::defaultORBFirstLevel()),
WTA_K_(Parameters::defaultORBWTA_K()),
scoreType_(Parameters::defaultORBScoreType()),
patchSize_(Parameters::defaultORBPatchSize()),
gpu_(Parameters::defaultORBGpu()),
fastThreshold_(Parameters::defaultFASTThreshold()),
nonmaxSuppresion_(Parameters::defaultFASTNonmaxSuppression()),
_orb(0),
_gpuOrb(0)
{
int nFeatures = Parameters::defaultORBNFeatures();
float scaleFactor = Parameters::defaultORBScaleFactor();
int nLevels = Parameters::defaultORBNLevels();
int edgeThreshold = Parameters::defaultORBEdgeThreshold();
int firstLevel = Parameters::defaultORBFirstLevel();
int WTA_K = Parameters::defaultORBWTA_K();
int scoreType = Parameters::defaultORBScoreType();
int patchSize = Parameters::defaultORBPatchSize();
bool gpu = Parameters::defaultORBGpu();
int fastThreshold = Parameters::defaultFASTThreshold();
bool nonmaxSuppresion = Parameters::defaultFASTNonmaxSuppression();
Parameters::parse(parameters, Parameters::kORBNFeatures(), nFeatures);
Parameters::parse(parameters, Parameters::kORBScaleFactor(), scaleFactor);
Parameters::parse(parameters, Parameters::kORBNLevels(), nLevels);
Parameters::parse(parameters, Parameters::kORBEdgeThreshold(), edgeThreshold);
Parameters::parse(parameters, Parameters::kORBFirstLevel(), firstLevel);
Parameters::parse(parameters, Parameters::kORBWTA_K(), WTA_K);
Parameters::parse(parameters, Parameters::kORBScoreType(), scoreType);
Parameters::parse(parameters, Parameters::kORBPatchSize(), patchSize);
Parameters::parse(parameters, Parameters::kORBGpu(), gpu);
Parameters::parse(parameters, Parameters::kFASTThreshold(), fastThreshold);
Parameters::parse(parameters, Parameters::kFASTNonmaxSuppression(), nonmaxSuppresion);
if(gpu && cv::gpu::getCudaEnabledDeviceCount())
{
_gpuOrb = new cv::gpu::ORB_GPU(nFeatures, scaleFactor, nLevels, edgeThreshold, firstLevel, WTA_K, scoreType, patchSize);
_gpuOrb->setFastParams(fastThreshold, nonmaxSuppresion);
}
else
{
if(gpu)
{
UWARN("GPU version of ORB not available! Using CPU version instead...");
}
_orb = new cv::ORB(nFeatures, scaleFactor, nLevels, edgeThreshold, firstLevel, WTA_K, scoreType, patchSize);
}
parseParameters(parameters);
}
ORB::~ORB()
@@ -444,6 +442,47 @@ ORB::~ORB()
}
}
void ORB::parseParameters(const ParametersMap & parameters)
{
Parameters::parse(parameters, Parameters::kORBNFeatures(), nFeatures_);
Parameters::parse(parameters, Parameters::kORBScaleFactor(), scaleFactor_);
Parameters::parse(parameters, Parameters::kORBNLevels(), nLevels_);
Parameters::parse(parameters, Parameters::kORBEdgeThreshold(), edgeThreshold_);
Parameters::parse(parameters, Parameters::kORBFirstLevel(), firstLevel_);
Parameters::parse(parameters, Parameters::kORBWTA_K(), WTA_K_);
Parameters::parse(parameters, Parameters::kORBScoreType(), scoreType_);
Parameters::parse(parameters, Parameters::kORBPatchSize(), patchSize_);
Parameters::parse(parameters, Parameters::kORBGpu(), gpu_);
Parameters::parse(parameters, Parameters::kFASTThreshold(), fastThreshold_);
Parameters::parse(parameters, Parameters::kFASTNonmaxSuppression(), nonmaxSuppresion_);
if(_gpuOrb)
{
delete _gpuOrb;
_gpuOrb = 0;
}
if(_orb)
{
delete _orb;
_orb = 0;
}
if(gpu_ && cv::gpu::getCudaEnabledDeviceCount())
{
_gpuOrb = new cv::gpu::ORB_GPU(nFeatures_, scaleFactor_, nLevels_, edgeThreshold_, firstLevel_, WTA_K_, scoreType_, patchSize_);
_gpuOrb->setFastParams(fastThreshold_, nonmaxSuppresion_);
}
else
{
if(gpu_)
{
UWARN("GPU version of ORB not available! Using CPU version instead...");
}
_orb = new cv::ORB(nFeatures_, scaleFactor_, nLevels_, edgeThreshold_, firstLevel_, WTA_K_, scoreType_, patchSize_);
}
}
std::vector<cv::KeyPoint> ORB::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi) const
{
UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
@@ -499,31 +538,14 @@ cv::Mat ORB::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyP
//FAST
//////////////////////////
FAST::FAST(const ParametersMap & parameters) :
threshold_(Parameters::defaultFASTThreshold()),
nonmaxSuppression_(Parameters::defaultFASTNonmaxSuppression()),
gpu_(Parameters::defaultFASTGpu()),
gpuKeypointsRatio_(Parameters::defaultFASTGpuKeypointsRatio()),
_fast(0),
_gpuFast(0)
{
int threshold = Parameters::defaultFASTThreshold();
bool nonmaxSuppression = Parameters::defaultFASTNonmaxSuppression();
bool gpu = Parameters::defaultFASTGpu();
double gpuKeypointsRatio = Parameters::defaultFASTGpuKeypointsRatio();
Parameters::parse(parameters, Parameters::kFASTThreshold(), threshold);
Parameters::parse(parameters, Parameters::kFASTNonmaxSuppression(), nonmaxSuppression);
Parameters::parse(parameters, Parameters::kFASTGpu(), gpu);
Parameters::parse(parameters, Parameters::kFASTGpuKeypointsRatio(), gpuKeypointsRatio);
if(gpu && cv::gpu::getCudaEnabledDeviceCount())
{
_gpuFast = new cv::gpu::FAST_GPU(threshold, nonmaxSuppression, gpuKeypointsRatio);
}
else
{
if(gpu)
{
UWARN("GPU version of FAST not available! Using CPU version instead...");
}
_fast = new cv::FastFeatureDetector(threshold, nonmaxSuppression);
}
parseParameters(parameters);
}
FAST::~FAST()
@@ -538,6 +560,38 @@ FAST::~FAST()
}
}
void FAST::parseParameters(const ParametersMap & parameters)
{
Parameters::parse(parameters, Parameters::kFASTThreshold(), threshold_);
Parameters::parse(parameters, Parameters::kFASTNonmaxSuppression(), nonmaxSuppression_);
Parameters::parse(parameters, Parameters::kFASTGpu(), gpu_);
Parameters::parse(parameters, Parameters::kFASTGpuKeypointsRatio(), gpuKeypointsRatio_);
if(_gpuFast)
{
delete _gpuFast;
_gpuFast = 0;
}
if(_fast)
{
delete _fast;
_fast = 0;
}
if(gpu_ && cv::gpu::getCudaEnabledDeviceCount())
{
_gpuFast = new cv::gpu::FAST_GPU(threshold_, nonmaxSuppression_, gpuKeypointsRatio_);
}
else
{
if(gpu_)
{
UWARN("GPU version of FAST not available! Using CPU version instead...");
}
_fast = new cv::FastFeatureDetector(threshold_, nonmaxSuppression_);
}
}
std::vector<cv::KeyPoint> FAST::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi) const
{
UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
@@ -560,11 +614,10 @@ std::vector<cv::KeyPoint> FAST::generateKeypointsImpl(const cv::Mat & image, con
//////////////////////////
FAST_BRIEF::FAST_BRIEF(const ParametersMap & parameters) :
FAST(parameters),
bytes_(Parameters::defaultBRIEFBytes()),
_brief(0)
{
int bytes = Parameters::defaultBRIEFBytes();
Parameters::parse(parameters, Parameters::kBRIEFBytes(), bytes);
_brief = new cv::BriefDescriptorExtractor(bytes);
parseParameters(parameters);
}
FAST_BRIEF::~FAST_BRIEF()
@@ -575,6 +628,19 @@ FAST_BRIEF::~FAST_BRIEF()
}
}
void FAST_BRIEF::parseParameters(const ParametersMap & parameters)
{
FAST::parseParameters(parameters);
Parameters::parse(parameters, Parameters::kBRIEFBytes(), bytes_);
if(_brief)
{
delete _brief;
_brief = 0;
}
_brief = new cv::BriefDescriptorExtractor(bytes_);
}
cv::Mat FAST_BRIEF::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{
UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
@@ -588,19 +654,13 @@ cv::Mat FAST_BRIEF::generateDescriptorsImpl(const cv::Mat & image, std::vector<c
//////////////////////////
FAST_FREAK::FAST_FREAK(const ParametersMap & parameters) :
FAST(parameters),
orientationNormalized_(Parameters::defaultFREAKOrientationNormalized()),
scaleNormalized_(Parameters::defaultFREAKScaleNormalized()),
patternScale_(Parameters::defaultFREAKPatternScale()),
nOctaves_(Parameters::defaultFREAKNOctaves()),
_freak(0)
{
bool orientationNormalized = Parameters::defaultFREAKOrientationNormalized();
bool scaleNormalized = Parameters::defaultFREAKScaleNormalized();
float patternScale = Parameters::defaultFREAKPatternScale();
int nOctaves = Parameters::defaultFREAKNOctaves();
Parameters::parse(parameters, Parameters::kFREAKOrientationNormalized(), orientationNormalized);
Parameters::parse(parameters, Parameters::kFREAKScaleNormalized(), scaleNormalized);
Parameters::parse(parameters, Parameters::kFREAKPatternScale(), patternScale);
Parameters::parse(parameters, Parameters::kFREAKNOctaves(), nOctaves);
_freak = new cv::FREAK(orientationNormalized, scaleNormalized, patternScale, nOctaves);
parseParameters(parameters);
}
FAST_FREAK::~FAST_FREAK()
@@ -611,6 +671,24 @@ FAST_FREAK::~FAST_FREAK()
}
}
void FAST_FREAK::parseParameters(const ParametersMap & parameters)
{
FAST::parseParameters(parameters);
Parameters::parse(parameters, Parameters::kFREAKOrientationNormalized(), orientationNormalized_);
Parameters::parse(parameters, Parameters::kFREAKScaleNormalized(), scaleNormalized_);
Parameters::parse(parameters, Parameters::kFREAKPatternScale(), patternScale_);
Parameters::parse(parameters, Parameters::kFREAKNOctaves(), nOctaves_);
if(_freak)
{
delete _freak;
_freak = 0;
}
_freak = new cv::FREAK(orientationNormalized_, scaleNormalized_, patternScale_, nOctaves_);
}
cv::Mat FAST_FREAK::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{
UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);

17
corelib/src/Memory.cpp
View File

@@ -63,8 +63,8 @@ Memory::Memory(const ParametersMap & parameters) :
_memoryChanged(false),
_signaturesAdded(0),
_feature2D(0),
_featureType(Feature2D::kFeatureUndef),
_feature2D(new SURF(parameters)),
_featureType(Feature2D::kFeatureSurf),
_badSignRatio(Parameters::defaultKpBadSignRatio()),
_tfIdfLikelihoodUsed(Parameters::defaultKpTfIdfLikelihoodUsed()),
_parallelized(Parameters::defaultKpParallelized()),
@@ -364,6 +364,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
}
//Keypoint detector
UASSERT(_feature2D != 0);
Feature2D::Type detectorStrategy = Feature2D::kFeatureUndef;
if((iter=parameters.find(Parameters::kKpDetectorStrategy())) != parameters.end())
{
@@ -403,6 +404,10 @@ void Memory::parseParameters(const ParametersMap & parameters)
break;
}
}
else if(_feature2D)
{
_feature2D->parseParameters(parameters);
}
}
void Memory::preUpdate()
@@ -1626,6 +1631,10 @@ Transform Memory::computeVisualTransform(int oldId, int newId) const
{
return computeVisualTransform(*oldS, *newS);
}
else
{
UWARN("Did not find nodes %d and/or %d", oldId, newId);
}
return Transform();
}
@@ -2845,6 +2854,10 @@ void Memory::extractKeypointsAndDescriptors(
filterKeypointsByDepth(keypoints, depth, fx, fy, cx, cy, _wordsMaxDepth);
limitKeypoints(keypoints, _wordsPerImageTarget);
}
else
{
UWARN("feature2D not set!");
}
if(keypoints.size())
{

50
corelib/src/util3d.cpp
View File

@@ -1173,7 +1173,7 @@ Transform transformFromXYZCorrespondences(
if(correspondencesInliers.size() == correspondences->size() && transform.isIdentity())
{
//Wrong transform
UDEBUG("Wrong transform: identity");
UINFO("Wrong transform: identity");
transform.setNull();
if(inliers)
{
@@ -1927,6 +1927,54 @@ std::map<int, Transform> radiusPosesFiltering(const std::map<int, Transform> & p
}
}
std::multimap<int, int> radiusPosesClustering(const std::map<int, Transform> & poses, float radius, float angle)
{
std::multimap<int, int> clusters;
if(poses.size() > 1 && radius > 0.0f && angle>0.0f)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
cloud->resize(poses.size());
int i=0;
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
(*cloud)[i++] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
}
// radius clustering (nearest neighbors)
std::vector<int> ids = uKeys(poses);
std::vector<Transform> transforms = uValues(poses);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> (false));
tree->setInputCloud(cloud);
for(unsigned int i=0; i<cloud->size(); ++i)
{
std::vector<int> kIndices;
std::vector<float> kDistances;
tree->radiusSearch(cloud->at(i), radius, kIndices, kDistances);
std::set<int> cloudIndices;
const Transform & currentT = transforms.at(i);
Eigen::Vector3f vA = util3d::transformToEigen3f(currentT).rotation()*Eigen::Vector3f(1,0,0);
for(unsigned int j=0; j<kIndices.size(); ++j)
{
if(i != kIndices[j])
{
const Transform & checkT = transforms.at(kIndices[j]);
// same orientation?
Eigen::Vector3f vB = util3d::transformToEigen3f(checkT).rotation()*Eigen::Vector3f(1,0,0);
double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
if(a <= angle)
{
clusters.insert(std::make_pair(ids[i], ids[kIndices[j]]));
}
}
}
}
}
return clusters;
}
/**
* Create 2d Occupancy grid (CV_8S)
* -1 = unknown

38
guilib/include/rtabmap/gui/DatabaseViewer.h
View File

@@ -44,7 +44,6 @@ namespace rtabmap
{
class Memory;
class ImageView;
}
class RTABMAPGUI_EXP DatabaseViewer : public QMainWindow
{
@@ -55,6 +54,9 @@ public:
virtual ~DatabaseViewer();
bool openDatabase(const QString & path);
protected:
virtual void resizeEvent(QResizeEvent* anEvent);
private slots:
void openDatabase();
void generateGraph();
@@ -64,6 +66,9 @@ private slots:
void generateTOROGraph();
void view3DMap();
void generate3DMap();
void detectMoreLoopClosures();
void refineAllNeighborLinks();
void refineAllLoopClosureLinks();
void sliderAValueChanged(int);
void sliderBValueChanged(int);
void sliderAMoved(int);
@@ -72,6 +77,10 @@ private slots:
void sliderLoopValueChanged(int);
void sliderIterationsValueChanged(int);
void updateGraphView();
void refineConstraint();
void addConstraint();
void resetConstraint();
void rejectConstraint();
private:
void updateIds();
@@ -81,7 +90,29 @@ private:
QLabel * labelChildren,
rtabmap::ImageView * view,
QLabel * labelId);
void updateWordsMatching();
void updateConstraintView(const rtabmap::Link & link);
void updateConstraintButtons();
std::multimap<int, Link>::iterator findLink(
std::multimap<int, Link> & links,
int from,
int to);
Link findActiveLink(int from, int to);
bool containsLink(
std::multimap<int, Link> & links,
int from,
int to);
std::multimap<int, rtabmap::Link> updateLinksWithModifications(
const std::multimap<int, rtabmap::Link> & edgeConstraints);
void updateLoopClosuresSlider(int from = 0, int to = 0);
void refineConstraint(int from, int to);
bool addConstraint(int from, int to, bool silent);
std::map<int, int> generateGraph(int fromNode, int margin);
std::map<int, Transform> optimizeGraph(
const std::map<int, int> & ids,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
std::list<std::map<int, rtabmap::Transform> > * graphes = 0);
private:
Ui_DatabaseViewer * ui_;
@@ -94,7 +125,12 @@ private:
std::list<std::map<int, rtabmap::Transform> > graphes_;
std::map<int, rtabmap::Transform> poses_;
std::multimap<int, rtabmap::Link> links_;
std::multimap<int, rtabmap::Link> linksRefined_;
std::multimap<int, rtabmap::Link> linksAdded_;
std::multimap<int, rtabmap::Link> linksRemoved_;
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > scans_;
};
}
#endif /* DATABASEVIEWER_H_ */

8
guilib/include/rtabmap/gui/ImageView.h
View File

@@ -24,6 +24,7 @@
#include <QtGui/QGraphicsView>
#include <QtCore/QRectF>
#include <QtCore/QMultiMap>
#include <opencv2/features2d/features2d.hpp>
#include <map>
@@ -57,7 +58,12 @@ public:
void setFeatures(const std::multimap<int, cv::KeyPoint> & refWords);
void setImage(const QImage & image);
void setImageDepth(const QImage & image);
void setFeatureColor(int id, const QColor & color);
void setFeaturesColor(const QColor & color);
const QMultiMap<int, rtabmap::KeypointItem *> & getFeatures() const {return _features;}
void clearLines();
void clear();
protected:
@@ -82,7 +88,7 @@ private:
QAction * _showLines;
QAction * _saveImage;
QList<rtabmap::KeypointItem *> _features;
QMultiMap<int, rtabmap::KeypointItem *> _features;
QGraphicsPixmapItem * _image;
QGraphicsPixmapItem * _imageDepth;
};

1314
guilib/src/DatabaseViewer.cpp
View File

File diff suppressed because it is too large Load Diff

60
guilib/src/ImageView.cpp
View File

@@ -87,9 +87,9 @@ bool ImageView::isFeaturesShown()
void ImageView::setFeaturesShown(bool shown)
{
_showFeatures->setChecked(shown);
for(int i=0; i<_features.size(); ++i)
for(QMultiMap<int, KeypointItem*>::iterator iter=_features.begin(); iter!=_features.end(); ++iter)
{
_features[i]->setVisible(_showFeatures->isChecked());
iter.value()->setVisible(_showFeatures->isChecked());
}
}
@@ -236,11 +236,7 @@ void ImageView::wheelEvent(QWheelEvent * e)
void ImageView::setFeatures(const std::multimap<int, cv::KeyPoint> & refWords)
{
for(int i=0; i<_features.size(); ++i)
{
scene()->removeItem(_features[i]);
delete _features[i];
}
qDeleteAll(_features);
_features.clear();
rtabmap::KeypointItem * item = 0;
@@ -261,7 +257,7 @@ void ImageView::setFeatures(const std::multimap<int, cv::KeyPoint> & refWords)
item = new rtabmap::KeypointItem(r.pt.x-radius, r.pt.y-radius, radius*2, info, QColor(255, 255, 0, alpha));
scene()->addItem(item);
_features.append(item);
_features.insert(id, item);
item->setVisible(_showFeatures->isChecked());
item->setZValue(1);
}
@@ -297,13 +293,51 @@ void ImageView::setImageDepth(const QImage & imageDepth)
}
}
void ImageView::setFeatureColor(int id, const QColor & color)
{
QList<KeypointItem*> items = _features.values(id);
if(items.size())
{
for(int i=0; i<items.size(); ++i)
{
items[i]->setPen(QPen(color));
items[i]->setBrush(QBrush(color));
}
}
else
{
UWARN("Not found feature %d", id);
}
}
void ImageView::setFeaturesColor(const QColor & color)
{
for(QMultiMap<int, KeypointItem*>::iterator iter=_features.begin(); iter!=_features.end(); ++iter)
{
iter.value()->setPen(QPen(color));
iter.value()->setBrush(QBrush(color));
}
}
void ImageView::clearLines()
{
if(this->scene())
{
QList<QGraphicsItem*> items = this->scene()->items();
for(int i=0; i<items.size(); ++i)
{
QGraphicsLineItem * line = qgraphicsitem_cast<QGraphicsLineItem*>(items[i]);
if(line)
{
delete line;
}
}
}
}
void ImageView::clear()
{
for(int i=0; i<_features.size(); ++i)
{
scene()->removeItem(_features[i]);
delete _features[i];
}
qDeleteAll(_features);
_features.clear();
if(_image)

468
guilib/src/ui/DatabaseViewer.ui
View File

@@ -7,7 +7,7 @@
<x>0</x>
<y>0</y>
<width>1076</width>
<height>637</height>
<height>582</height>
</rect>
</property>
<property name="windowTitle">
@@ -260,7 +260,7 @@
<x>0</x>
<y>0</y>
<width>1076</width>
<height>22</height>
<height>25</height>
</rect>
</property>
<widget class="QMenu" name="menuFile">
@@ -282,6 +282,10 @@
<addaction name="actionGenerate_local_graph_dot"/>
<addaction name="actionGenerate_TORO_graph_graph"/>
<addaction name="separator"/>
<addaction name="actionDetect_more_loop_closures"/>
<addaction name="actionRefine_all_neighbor_links"/>
<addaction name="actionRefine_all_loop_closure_links"/>
<addaction name="separator"/>
<addaction name="actionClean_database"/>
<addaction name="actionClean_local_graph"/>
<addaction name="separator"/>
@@ -309,13 +313,6 @@
<item>
<widget class="rtabmap::CloudViewer" name="constraintsViewer" native="true"/>
</item>
<item>
<widget class="QLabel" name="label_constraint">
<property name="text">
<string/>
</property>
</widget>
</item>
<item>
<layout class="QFormLayout" name="formLayout">
<property name="fieldGrowthPolicy">
@@ -355,6 +352,79 @@
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_constraint">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_16">
<property name="text">
<string>Transform</string>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QLabel" name="label_18">
<property name="text">
<string>Fitness</string>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_fitness">
<property name="text">
<string>-</string>
</property>
</widget>
</item>
</layout>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_5">
<item>
<widget class="QPushButton" name="pushButton_refine">
<property name="text">
<string>Refine (ICP)</string>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="pushButton_reset">
<property name="text">
<string>Reset</string>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="pushButton_add">
<property name="text">
<string>Add</string>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="pushButton_reject">
<property name="text">
<string>Reject</string>
</property>
</widget>
</item>
<item>
<spacer name="horizontalSpacer">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
</layout>
</item>
</layout>
@@ -464,6 +534,371 @@
</layout>
</widget>
</widget>
<widget class="QDockWidget" name="dockWidget_icp">
<property name="windowTitle">
<string>ICP parameters</string>
</property>
<attribute name="dockWidgetArea">
<number>2</number>
</attribute>
<widget class="QWidget" name="dockWidgetContents_3">
<layout class="QVBoxLayout" name="verticalLayout_11">
<item>
<layout class="QGridLayout" name="gridLayout_4" columnminimumwidth="1,0">
<item row="0" column="0">
<widget class="QLabel" name="label_14">
<property name="text">
<string>Decimation</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QSpinBox" name="spinBox_icp_decimation">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>8</number>
</property>
<property name="value">
<number>4</number>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_17">
<property name="text">
<string>Max depth</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox_icp_maxDepth">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="maximum">
<double>100.000000000000000</double>
</property>
<property name="value">
<double>2.000000000000000</double>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_13">
<property name="text">
<string>Voxel</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox_icp_voxel">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>3</number>
</property>
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="value">
<double>0.005000000000000</double>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QLabel" name="label_12">
<property name="text">
<string>Max correspondence distance</string>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox_icp_maxCorrespDistance">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>3</number>
</property>
<property name="minimum">
<double>0.001000000000000</double>
</property>
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="value">
<double>0.050000000000000</double>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QLabel" name="label_15">
<property name="text">
<string>Iteration</string>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QSpinBox" name="spinBox_icp_iteration">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>1000</number>
</property>
<property name="value">
<number>30</number>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QLabel" name="label_11">
<property name="text">
<string>Point to plane</string>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QCheckBox" name="checkBox_icp_p2plane">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QLabel" name="label_20">
<property name="text">
<string>Normal K neighbors</string>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QSpinBox" name="spinBox_icp_normalKSearch">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>1000</number>
</property>
<property name="value">
<number>20</number>
</property>
</widget>
</item>
</layout>
</item>
<item>
<spacer name="verticalSpacer_2">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>31</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
</widget>
<widget class="QDockWidget" name="dockWidget_visual">
<property name="windowTitle">
<string>Visual parameters</string>
</property>
<attribute name="dockWidgetArea">
<number>1</number>
</attribute>
<widget class="QWidget" name="dockWidgetContents_4">
<layout class="QVBoxLayout" name="verticalLayout_10">
<item>
<layout class="QGridLayout" name="gridLayout_3" columnminimumwidth="1,0">
<item row="0" column="0">
<widget class="QLabel" name="label_24">
<property name="text">
<string>Recompute features (SURF)</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QCheckBox" name="checkBox_visual_recomputeFeatures">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_25">
<property name="text">
<string>SURF hessian threshold</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox_visual_hessian">
<property name="suffix">
<string/>
</property>
<property name="decimals">
<number>0</number>
</property>
<property name="minimum">
<double>10.000000000000000</double>
</property>
<property name="maximum">
<double>100000.000000000000000</double>
</property>
<property name="value">
<double>150.000000000000000</double>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_26">
<property name="text">
<string>NNDR</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox_visual_nndr">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>1</number>
</property>
<property name="minimum">
<double>0.100000000000000</double>
</property>
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>0.800000000000000</double>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QLabel" name="label_21">
<property name="text">
<string>Min correspondences</string>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QSpinBox" name="spinBox_visual_minCorrespondences">
<property name="minimum">
<number>3</number>
</property>
<property name="maximum">
<number>1000</number>
</property>
<property name="value">
<number>3</number>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QLabel" name="label_22">
<property name="text">
<string>Max correspondence distance</string>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox_visual_maxCorrespDistance">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>3</number>
</property>
<property name="minimum">
<double>0.001000000000000</double>
</property>
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="value">
<double>0.010000000000000</double>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QLabel" name="label_23">
<property name="text">
<string>Iteration</string>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QSpinBox" name="spinBox_visual_iteration">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>1000</number>
</property>
<property name="value">
<number>30</number>
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QLabel" name="label_19">
<property name="text">
<string>Max feature depth</string>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox_visual_maxDepth">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="maximum">
<double>100.000000000000000</double>
</property>
<property name="value">
<double>4.000000000000000</double>
</property>
</widget>
</item>
</layout>
</item>
<item>
<spacer name="verticalSpacer">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>40</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
</widget>
<action name="actionOpen_database">
<property name="text">
<string>Open database</string>
@@ -529,6 +964,21 @@
<string>Generate TORO graph (*.graph)...</string>
</property>
</action>
<action name="actionRefine_all_neighbor_links">
<property name="text">
<string>Refine all neighbor links...</string>
</property>
</action>
<action name="actionRefine_all_loop_closure_links">
<property name="text">
<string>Refine all loop closure links...</string>
</property>
</action>
<action name="actionDetect_more_loop_closures">
<property name="text">
<string>Detect more loop closures...</string>
</property>
</action>
</widget>
<customwidgets>
<customwidget>

2
tools/DatabaseViewer/main.cpp
View File

@@ -27,7 +27,7 @@ int main(int argc, char * argv[])
ULogger::setLevel(ULogger::kInfo);
QApplication * app = new QApplication(argc, argv);
DatabaseViewer * mainWindow = new DatabaseViewer();
rtabmap::DatabaseViewer * mainWindow = new rtabmap::DatabaseViewer();
mainWindow->showNormal();

2
tools/VocabularyComparison/main.cpp
View File

@@ -77,7 +77,7 @@ int main(int argc, char * argv[])
{
std::getline(file, str);
strList = uSplit(str);
if(strList.size() == dimension+1)
if((int)strList.size() == dimension+1)
{
//first one is the visual word id
std::list<std::string>::iterator iter = strList.begin();