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

Refactored OdometryBOW class to handle binary descriptors too (removed OdometryBin).

Browse Source 
Odometry: added "local map history" option to increase precision
Added new Nearest neighbor options (LSH, brute Force, GPU brute Force)
Added FREAK/ORB features
Increased version to 0.6.5

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1431 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-06-22 03:33:56 +00:00
parent bff3acf273
commit 65b79cd7a2
30 changed files with 2812 additions and 3045 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+2 -2
View File
@@ -16,7 +16,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
#######################
SET(RTABMAP_MAJOR_VERSION 0)
SET(RTABMAP_MINOR_VERSION 6)
SET(RTABMAP_PATCH_VERSION 4)
SET(RTABMAP_PATCH_VERSION 5)
SET(RTABMAP_VERSION
${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
@@ -132,7 +132,7 @@ IF(APPLE)
ENDIF(APPLE)
####### DEPENDENCIES #######
FIND_PACKAGE(OpenCV REQUIRED core highgui flann features2d calib3d imgproc nonfree)
FIND_PACKAGE(OpenCV REQUIRED core highgui flann features2d calib3d imgproc nonfree gpu)
FIND_PACKAGE(PCL 1.7 REQUIRED)
FIND_PACKAGE(VTK REQUIRED)
FIND_PACKAGE(ZLIB REQUIRED)
corelib/include/rtabmap/core/CameraEvent.h
+2 -2
View File
@@ -44,9 +44,9 @@ public:
_image(image, seq)
{
}
CameraEvent(const cv::Mat & descriptors, const std::vector<cv::KeyPoint> & keypoints, const cv::Mat & image = cv::Mat(), int seq=0) :
CameraEvent(const cv::Mat & descriptors, Feature2D::Type featureType, const std::vector<cv::KeyPoint> & keypoints, const cv::Mat & image = cv::Mat(), int seq=0) :
UEvent(kCodeFeatures),
_image(image, seq, descriptors, keypoints)
_image(image, seq, descriptors, featureType, keypoints)
{
}
CameraEvent() :
corelib/include/rtabmap/core/Features2d.h
+84 -78
View File
@@ -28,6 +28,16 @@
#include <list>
#include "rtabmap/core/Parameters.h"
namespace cv{
class SURF;
class SIFT;
namespace gpu {
class SURF_GPU;
class ORB_GPU;
class FAST_GPU;
}
}
namespace rtabmap {
void RTABMAP_EXP filterKeypointsByDepth(
@@ -45,119 +55,115 @@ void RTABMAP_EXP filterKeypointsByDepth(
void RTABMAP_EXP limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints);
void RTABMAP_EXP limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints);
/////////////////////
// KeypointDescriptor
/////////////////////
class RTABMAP_EXP KeypointDescriptor {
cv::Rect RTABMAP_EXP computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios);
// Feature2D
class RTABMAP_EXP Feature2D {
public:
enum DescriptorType {kDescriptorSurf, kDescriptorSift, kDescriptorUndef};
enum Type {kFeatureUndef=-1, kFeatureSurf=0, kFeatureSift=1, kFeatureOrb=2, kFeatureFastFreak=3, kFeatureFastBrief=4};
public:
virtual ~KeypointDescriptor();
virtual void parseParameters(const ParametersMap & parameters);
virtual cv::Mat generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const = 0;
virtual ~Feature2D() {}
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;
protected:
KeypointDescriptor(const ParametersMap & parameters = ParametersMap());
Feature2D(const ParametersMap & parameters = ParametersMap()) {}
private:
virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi) const = 0;
virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const = 0;
};
//SURFDescriptor
class RTABMAP_EXP SURFDescriptor : public KeypointDescriptor
//SURF
class RTABMAP_EXP SURF : public Feature2D
{
public:
SURFDescriptor(const ParametersMap & parameters = ParametersMap());
virtual ~SURFDescriptor();
virtual void parseParameters(const ParametersMap & parameters);
virtual cv::Mat generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
SURF(const ParametersMap & parameters = ParametersMap());
virtual ~SURF();
private:
double _hessianThreshold;
int _nOctaves;
int _nOctaveLayers;
bool _extended;
bool _upright;
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;
bool _gpuVersion;
private:
cv::SURF * _surf;
cv::gpu::SURF_GPU * _gpuSurf;
};
//SIFTDescriptor
class RTABMAP_EXP SIFTDescriptor : public KeypointDescriptor
//SIFT
class RTABMAP_EXP SIFT : public Feature2D
{
public:
SIFTDescriptor(const ParametersMap & parameters = ParametersMap());
virtual ~SIFTDescriptor();
virtual void parseParameters(const ParametersMap & parameters);
virtual cv::Mat generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
SIFT(const ParametersMap & parameters = ParametersMap());
virtual ~SIFT();
private:
int _nfeatures;
int _nOctaveLayers;
double _contrastThreshold;
double _edgeThreshold;
double _sigma;
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:
cv::SIFT * _sift;
};
/////////////////////
// KeypointDetector
/////////////////////
class RTABMAP_EXP KeypointDetector
//ORB
class RTABMAP_EXP ORB : public Feature2D
{
public:
enum DetectorType {kDetectorSurf, kDetectorSift, kDetectorUndef};
ORB(const ParametersMap & parameters = ParametersMap());
virtual ~ORB();
public:
static cv::Rect computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios);
public:
virtual ~KeypointDetector() {}
std::vector<cv::KeyPoint> generateKeypoints(
const cv::Mat & image,
int maxKeypoints = 0,
const cv::Rect & roi = cv::Rect());
virtual void parseParameters(const ParametersMap & parameters);
void setRoi(const std::string & roi);
protected:
KeypointDetector(const ParametersMap & parameters = ParametersMap());
private:
virtual std::vector<cv::KeyPoint> _generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const = 0;
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:
cv::ORB * _orb;
cv::gpu::ORB_GPU * _gpuOrb;
};
//SURFDetector
class RTABMAP_EXP SURFDetector : public KeypointDetector
//FAST
class RTABMAP_EXP FAST : public Feature2D
{
public:
SURFDetector(const ParametersMap & parameters = ParametersMap());
virtual ~SURFDetector();
virtual void parseParameters(const ParametersMap & parameters);
private:
virtual std::vector<cv::KeyPoint> _generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const;
private:
double _hessianThreshold;
int _nOctaves;
int _nOctaveLayers;
bool _extended;
bool _upright;
FAST(const ParametersMap & parameters = ParametersMap());
virtual ~FAST();
bool _gpuVersion;
private:
virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi) const;
private:
cv::FastFeatureDetector * _fast;
cv::gpu::FAST_GPU * _gpuFast;
};
//SIFTDetector
class RTABMAP_EXP SIFTDetector : public KeypointDetector
//FAST_BRIEF
class RTABMAP_EXP FAST_BRIEF : public FAST
{
public:
SIFTDetector(const ParametersMap & parameters = ParametersMap());
virtual ~SIFTDetector();
virtual void parseParameters(const ParametersMap & parameters);
FAST_BRIEF(const ParametersMap & parameters = ParametersMap());
virtual ~FAST_BRIEF();
private:
virtual std::vector<cv::KeyPoint> _generateKeypoints(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::BriefDescriptorExtractor * _brief;
};
//FAST_FREAK
class RTABMAP_EXP FAST_FREAK : public FAST
{
public:
FAST_FREAK(const ParametersMap & parameters = ParametersMap());
virtual ~FAST_FREAK();
private:
virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
private:
cv::FREAK * _freak;
};
}
corelib/include/rtabmap/core/Image.h
+8 -1
View File
@@ -12,6 +12,7 @@
#include <opencv2/features2d/features2d.hpp>
#include <rtabmap/core/Transform.h>
#include <rtabmap/core/Features2d.h>
namespace rtabmap
{
@@ -25,10 +26,12 @@ public:
Image(const cv::Mat & image = cv::Mat(),
int id = 0,
const cv::Mat & descriptors = cv::Mat(),
Feature2D::Type featureType = Feature2D::kFeatureUndef,
const std::vector<cv::KeyPoint> & keypoints = std::vector<cv::KeyPoint>()) :
_image(image),
_id(id),
_descriptors(descriptors),
_featureType(featureType),
_keypoints(keypoints),
_depthConstant(0.0f),
_localTransform(Transform::getIdentity())
@@ -44,6 +47,7 @@ public:
int id = 0) :
_image(image),
_id(id),
_featureType(Feature2D::kFeatureUndef),
_depth(depth),
_depthConstant(depthConstant),
_pose(pose),
@@ -61,6 +65,7 @@ public:
int id = 0) :
_image(image),
_id(id),
_featureType(Feature2D::kFeatureUndef),
_depth(depth),
_depth2d(depth2d),
_depthConstant(depthConstant),
@@ -75,8 +80,9 @@ public:
const cv::Mat & image() const {return _image;}
int id() const {return _id;};
const cv::Mat & descriptors() const {return _descriptors;}
Feature2D::Type featureType() const {return _featureType;}
const std::vector<cv::KeyPoint> & keypoints() const {return _keypoints;}
void setDescriptors(const cv::Mat & descriptors) {_descriptors = descriptors;}
void setDescriptors(const cv::Mat & descriptors, Feature2D::Type featureType) {_descriptors = descriptors; _featureType=featureType;}
void setKeypoints(const std::vector<cv::KeyPoint> & keypoints) {_keypoints = keypoints;}
bool isMetric() const {return !_depth.empty() || _depthConstant != 0.0f || !_pose.isNull();}
@@ -91,6 +97,7 @@ private:
cv::Mat _image;
int _id;
cv::Mat _descriptors;
Feature2D::Type _featureType;
std::vector<cv::KeyPoint> _keypoints;
// Metric stuff
corelib/include/rtabmap/core/Memory.h
+9 -8
View File
@@ -41,8 +41,7 @@ class DBDriver;
class GraphNode;
class VWDictionary;
class VisualWord;
class KeypointDetector;
class KeypointDescriptor;
class Feature2D;
class Statistics;
class RTABMAP_EXP Memory
@@ -80,7 +79,7 @@ public:
bool incrementMarginOnLoop = false,
bool ignoreLoopIds = false,
double * dbAccessTime = 0) const;
void deleteLocation(int locationId);
void deleteLocation(int locationId, std::list<int> * deletedWords = 0);
void rejectLoopClosure(int oldId, int newId);
//getters
@@ -135,8 +134,9 @@ public:
const std::set<int> & endIds = std::set<int>());
//keypoint stuff
int getVWDictionarySize() const;
const VWDictionary * getVWDictionary() const;
std::multimap<int, cv::KeyPoint> getWords(int signatureId) const;
Feature2D::Type getFeatureType() const {return _featureType;}
void extractKeypointsAndDescriptors(
const cv::Mat & image,
const cv::Mat & depth,
@@ -144,6 +144,7 @@ public:
std::vector<cv::KeyPoint> & keypoints,
cv::Mat & descriptors);
// RGB-D stuff
void getMetricConstraints(
const std::vector<int> & ids,
std::map<int, Transform> & poses,
@@ -162,7 +163,7 @@ private:
void preUpdate();
void addSignatureToStm(Signature * signature);
void clear();
void moveToTrash(Signature * s, bool saveToDatabase = true);
void moveToTrash(Signature * s, bool saveToDatabase = true, std::list<int> * deletedWords = 0);
void addSignatureToWm(Signature * signature);
Signature * _getSignature(int id) const;
@@ -181,7 +182,7 @@ private:
bool keepRawData=false);
//keypoint stuff
void disableWordsRef(int signatureId, bool saveToDatabase = true);
void disableWordsRef(int signatureId);
void enableWordsRef(const std::list<int> & signatureIds);
void cleanUnusedWords();
int getNi(int signatureId) const;
@@ -215,8 +216,8 @@ private:
//Keypoint stuff
VWDictionary * _vwd;
KeypointDetector * _keypointDetector;
KeypointDescriptor * _keypointDescriptor;
Feature2D * _feature2D;
Feature2D::Type _featureType;
float _badSignRatio;;
bool _tfIdfLikelihoodUsed;
bool _parallelized;
corelib/include/rtabmap/core/Odometry.h
+12 -70
View File
@@ -40,6 +40,7 @@ public:
bool isLargeEnoughTransform(const Transform & transform);
//getters
const Transform & getPose() const {return _pose;}
int getMaxFeatures() const {return _maxFeatures;}
int getMinInliers() const {return _minInliers;}
float getInlierDistance() const {return _inlierDistance;}
@@ -48,6 +49,7 @@ public:
float getMaxDepth() const {return _maxDepth;}
float geLinearUpdate() const {return _linearUpdate;}
float getAngularUpdate() const {return _angularUpdate;}
int getLocalHistory() const {return _localHistory;}
private:
virtual Transform computeTransform(Image & image, int * quality = 0) = 0;
@@ -62,76 +64,20 @@ private:
float _linearUpdate;
float _angularUpdate;
int _resetCountdown;
int _localHistory;
Transform _pose;
int _resetCurrentCount;
protected:
Odometry(float inlierDistance = Parameters::defaultOdomInlierDistance(),
int maxWords = Parameters::defaultOdomMaxWords(),
int minInliers = Parameters::defaultOdomMinInliers(),
int iterations = Parameters::defaultOdomIterations(),
float wordsRatio = Parameters::defaultOdomWordsRatio(),
float maxDepth = Parameters::defaultOdomMaxDepth(),
float linearUpdate = Parameters::defaultOdomLinearUpdate(),
float angularUpdate = Parameters::defaultOdomAngularUpdate(),
int resetCountDown = Parameters::defaultOdomResetCountdown());
Odometry(const rtabmap::ParametersMap & parameters);
};
class RTABMAP_EXP OdometryBinary : public Odometry
{
public:
OdometryBinary(
float inlierDistance = Parameters::defaultOdomInlierDistance(),
int maxWords = Parameters::defaultOdomMaxWords(),
int minInliers = Parameters::defaultOdomMinInliers(),
int iterations = Parameters::defaultOdomIterations(),
float wordsRatio = Parameters::defaultOdomWordsRatio(),
float maxDepth = Parameters::defaultOdomMaxDepth(),
float linearUpdate = Parameters::defaultOdomLinearUpdate(),
float angularUpdate = Parameters::defaultOdomAngularUpdate(),
int resetCountdown = Parameters::defaultOdomResetCountdown(),
int briefBytes = Parameters::defaultOdomBinBriefBytes(),
int fastThreshold = Parameters::defaultOdomBinFastThreshold(),
bool fastNonmaxSuppression = Parameters::defaultOdomBinFastNonmaxSuppression(),
bool bruteForceMatching = Parameters::defaultOdomBinBruteForceMatching());
OdometryBinary(const rtabmap::ParametersMap & parameters);
virtual ~OdometryBinary() {}
virtual void reset();
private:
virtual Transform computeTransform(Image & image, int * quality = 0);
private:
int _briefBytes;
int _fastThreshold;
bool _fastNonmaxSuppression;
bool _bruteForceMatching;
std::vector<cv::KeyPoint> _lastKeypoints;
cv::Mat _lastDescriptors;
cv::Mat _lastDepth;
};
class Memory;
class RTABMAP_EXP OdometryBOW : public Odometry
{
public:
OdometryBOW(
int detectorType = Parameters::defaultKpDetectorStrategy(), // 0=SURF or 1=SIFT
float inlierDistance = Parameters::defaultOdomInlierDistance(),
int maxWords = Parameters::defaultOdomMaxWords(),
int minInliers = Parameters::defaultOdomMinInliers(),
int iterations = Parameters::defaultOdomIterations(),
float wordsRatio = Parameters::defaultOdomWordsRatio(),
float maxDepth = Parameters::defaultOdomMaxDepth(),
float linearUpdate = Parameters::defaultOdomLinearUpdate(),
float angularUpdate = Parameters::defaultOdomAngularUpdate(),
int resetCoutdown = Parameters::defaultOdomResetCountdown(),
float surfHessianThreshold = Parameters::defaultSURFHessianThreshold(),
float nndr = Parameters::defaultKpNndrRatio()); // nearest neighbor distance ratio
OdometryBOW(const rtabmap::ParametersMap & parameters);
OdometryBOW(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
virtual ~OdometryBOW();
virtual void reset();
@@ -141,23 +87,19 @@ private:
private:
Memory * _memory;
std::multimap<int, pcl::PointXYZ> localMap_;
};
class RTABMAP_EXP OdometryICP : public Odometry
{
public:
OdometryICP(
int decimation = Parameters::defaultOdomICPDecimation(),
float voxelSize = Parameters::defaultOdomICPVoxelSize(),
float samples = Parameters::defaultOdomICPSamples(),
float maxCorrespondenceDistance = Parameters::defaultOdomICPCorrespondencesDistance(),
int maxIterations = Parameters::defaultOdomICPIterations(),
float maxFitness = Parameters::defaultOdomICPMaxFitness(),
float maxDepth = Parameters::defaultOdomMaxDepth(),
float linearUpdate = Parameters::defaultOdomLinearUpdate(),
float angularUpdate = Parameters::defaultOdomAngularUpdate(),
int resetCoutdown = Parameters::defaultOdomResetCountdown());
OdometryICP(const ParametersMap & parameters);
OdometryICP(int decimation = 4,
float voxelSize = 0.005f,
int samples = 0,
float maxCorrespondenceDistance = 0.05f,
int maxIterations = 30,
float maxFitness = 0.01f,
const ParametersMap & odometryParameter = rtabmap::ParametersMap());
void reset();
private:
corelib/include/rtabmap/core/Parameters.h
+30 -20
View File
@@ -153,17 +153,13 @@ class RTABMAP_EXP Parameters
// KeypointMemory (Keypoint-based)
RTABMAP_PARAM(Kp, PublishKeypoints, bool, true, "Publishing keypoints.");
RTABMAP_PARAM(Kp, NNStrategy, int, 1, "Naive 0, kdForest 1.");
RTABMAP_PARAM(Kp, NNStrategy, int, 1, "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
RTABMAP_PARAM(Kp, IncrementalDictionary, bool, true, "");
RTABMAP_PARAM(Kp, MaxDepth, float, 0.0, "Filter extracted keypoints by depth (0=inf)");
RTABMAP_PARAM(Kp, WordsPerImage, int, 400, "");
RTABMAP_PARAM(Kp, BadSignRatio, float, 0.2, "Bad signature ratio (less than Ratio x AverageWordsPerImage = bad).");
RTABMAP_PARAM(Kp, MinDistUsed, bool, false, "The nearest neighbor must have a distance < minDist.");
RTABMAP_PARAM(Kp, MinDist, float, 0.05, "Matching a descriptor with a word (euclidean distance ^ 2)");
RTABMAP_PARAM(Kp, NndrUsed, bool, true, "If NNDR ratio is used.");
RTABMAP_PARAM(Kp, NndrRatio, float, 0.8, "NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)");
RTABMAP_PARAM(Kp, MaxLeafs, int, 64, "Maximum number of leafs checked (when using kd-trees).");
RTABMAP_PARAM(Kp, DetectorStrategy, int, 0, "Surf detector 0, SIFT detector 1, undef 2.");
RTABMAP_PARAM(Kp, DetectorStrategy, int, 0, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF.");
RTABMAP_PARAM(Kp, TfIdfLikelihoodUsed, bool, false, "Use of the td-idf strategy to compute the likelihood.");
RTABMAP_PARAM(Kp, Parallelized, bool, true, "If the dictionary update and signature creation were parallelized.");
RTABMAP_PARAM_STR(Kp, RoiRatios, "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
@@ -183,6 +179,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(SURF, OctaveLayers, int, 2, "");
RTABMAP_PARAM(SURF, Upright, bool, false, "U-SURF");
RTABMAP_PARAM(SURF, GpuVersion, bool, false, "");
RTABMAP_PARAM(SURF, GpuKeypointsRatio, float, 0.01, "");
RTABMAP_PARAM(SIFT, NFeatures, int, 0, "");
RTABMAP_PARAM(SIFT, NOctaveLayers, int, 3, "");
@@ -190,6 +187,28 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(SIFT, EdgeThreshold, double, 10.0, "");
RTABMAP_PARAM(SIFT, Sigma, double, 1.6, "");
RTABMAP_PARAM(BRIEF, Bytes, int, 32, "Bytes is a length of descriptor in bytes. It can be equal 16, 32 or 64 bytes.");
RTABMAP_PARAM(FAST, Threshold, int, 30, "Threshold on difference between intensity of the central pixel and pixels of a circle around this pixel.");
RTABMAP_PARAM(FAST, NonmaxSuppression, bool, true, "If true, non-maximum suppression is applied to detected corners (keypoints).");
RTABMAP_PARAM(FAST, Gpu, bool, false, "GPU-FAST: Use GPU version of FAST. This option is enabled only if OpenCV is built with CUDA and GPUs are detected.");
RTABMAP_PARAM(FAST, GpuKeypointsRatio, double, 0.05, "Used with FAST GPU.");
RTABMAP_PARAM(ORB, NFeatures, int, 500, "The maximum number of features to retain.");
RTABMAP_PARAM(ORB, ScaleFactor, float, 1.2, "Pyramid decimation ratio, greater than 1. scaleFactor==2 means the classical pyramid, where each next level has 4x less pixels than the previous, but such a big scale factor will degrade feature matching scores dramatically. On the other hand, too close to 1 scale factor will mean that to cover certain scale range you will need more pyramid levels and so the speed will suffer.");
RTABMAP_PARAM(ORB, NLevels, int, 8, "The number of pyramid levels. The smallest level will have linear size equal to input_image_linear_size/pow(scaleFactor, nlevels).");
RTABMAP_PARAM(ORB, EdgeThreshold, int, 31, "This is size of the border where the features are not detected. It should roughly match the patchSize parameter.");
RTABMAP_PARAM(ORB, FirstLevel, int, 0, "It should be 0 in the current implementation.");
RTABMAP_PARAM(ORB, WTA_K, int, 2, "The number of points that produce each element of the oriented BRIEF descriptor. The default value 2 means the BRIEF where we take a random point pair and compare their brightnesses, so we get 0/1 response. Other possible values are 3 and 4. For example, 3 means that we take 3 random points (of course, those point coordinates are random, but they are generated from the pre-defined seed, so each element of BRIEF descriptor is computed deterministically from the pixel rectangle), find point of maximum brightness and output index of the winner (0, 1 or 2). Such output will occupy 2 bits, and therefore it will need a special variant of Hamming distance, denoted as NORM_HAMMING2 (2 bits per bin). When WTA_K=4, we take 4 random points to compute each bin (that will also occupy 2 bits with possible values 0, 1, 2 or 3).");
RTABMAP_PARAM(ORB, ScoreType, int, 0, "The default HARRIS_SCORE=0 means that Harris algorithm is used to rank features (the score is written to KeyPoint::score and is used to retain best nfeatures features); FAST_SCORE=1 is alternative value of the parameter that produces slightly less stable keypoints, but it is a little faster to compute.");
RTABMAP_PARAM(ORB, PatchSize, int, 31, "size of the patch used by the oriented BRIEF descriptor. Of course, on smaller pyramid layers the perceived image area covered by a feature will be larger.");
RTABMAP_PARAM(ORB, Gpu, bool, false, "GPU-ORB: Use GPU version of ORB. This option is enabled only if OpenCV is built with CUDA and GPUs are detected.");
RTABMAP_PARAM(FREAK, OrientationNormalized, bool, true, "Enable orientation normalization.");
RTABMAP_PARAM(FREAK, ScaleNormalized, bool, true, "Enable scale normalization.");
RTABMAP_PARAM(FREAK, PatternScale, float, 22.0, "Scaling of the description pattern.");
RTABMAP_PARAM(FREAK, NOctaves, int, 4, "Number of octaves covered by the detected keypoints.");
// BayesFilter
RTABMAP_PARAM(Bayes, VirtualPlacePriorThr, float, 0.9, "Virtual place prior");
RTABMAP_PARAM_STR(Bayes, PredictionLC, "0.1 0.36 0.30 0.16 0.062 0.0151 0.00255 0.000324 2.5e-05 1.3e-06 4.8e-08 1.2e-09 1.9e-11 2.2e-13 1.7e-15 8.5e-18 2.9e-20 6.9e-23", "Prediction of loop closures (Gaussian-like, here with sigma=1.6) - Format: {VirtualPlaceProb, LoopClosureProb, NeighborLvl1, NeighborLvl2, ...}.");
@@ -216,7 +235,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(RGBD, LocalLoopDetectionMaxDiffID, int, 0, "Maximum ID difference between the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.")
// Odometry
RTABMAP_PARAM(Odom, Type, int, 0, "0=BOW 1=Binary.");
RTABMAP_PARAM(Odom, Type, int, 0, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF.");
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, MaxWords, int, 0, "0 no limits.");
@@ -225,19 +244,10 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Odom, Iterations, int, 100, "Maximum iterations to compute the transform from visual words.");
RTABMAP_PARAM(Odom, MaxDepth, float, 5.0, "Max depth of the words (0 means no limit).");
RTABMAP_PARAM(Odom, WordsRatio, float, 0.5, "Minmum ratio of keypoints between the current image and the last image to compute odometry.");
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(OdomBin, BriefBytes, int, 32, "");
RTABMAP_PARAM(OdomBin, FastThreshold, int, 30, "");
RTABMAP_PARAM(OdomBin, FastNonmaxSuppression, bool, true, "");
RTABMAP_PARAM(OdomBin, BruteForceMatching, bool, true, "If false, FLANN LSH is used.");
RTABMAP_PARAM(OdomICP, Decimation, int, 4, "");
RTABMAP_PARAM(OdomICP, VoxelSize, float, 0.005, "Voxel size to be used for ICP computation.");
RTABMAP_PARAM(OdomICP, Samples, int, 0, "not used if voxelSize is set.");
RTABMAP_PARAM(OdomICP, CorrespondencesDistance, float, 0.05, "");
RTABMAP_PARAM(OdomICP, Iterations, int, 30, "");
RTABMAP_PARAM(OdomICP, MaxFitness, float, 0.01, "");
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(Odom, LocalHistory, int, 0, "Local history size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
RTABMAP_PARAM(Odom, NearestNeighbor, int, 1, "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
RTABMAP_PARAM(Odom, NNDR, float, 0.7, "NNDR: nearest neighbor distance ratio.");
// Loop closure constraint
RTABMAP_PARAM(LccIcp, Type, int, 0, "0=No ICP, 1=ICP 3D, 2=ICP 2D");
corelib/include/rtabmap/core/VWDictionary.h
+8 -19
View File
@@ -31,14 +31,13 @@
namespace rtabmap
{
class FlannNN;
class DBDriver;
class VisualWord;
class RTABMAP_EXP VWDictionary
{
public:
enum NNStrategy{kNNNaive, kNNFlannKdTree, kNNUndef};
enum NNStrategy{kNNFlannNaive, kNNFlannKdTree, kNNFlannLSH, kNNBruteForce, kNNBruteForceGPU, kNNUndef};
static const int ID_START;
static const int ID_INVALID;
@@ -55,34 +54,28 @@ public:
int signatureId);
virtual void addWord(VisualWord * vw);
virtual std::vector<int> findNN(const std::list<VisualWord *> & vws, bool searchInNewlyAddedWords = true) const;
void naiveNNSearch(const std::list<VisualWord *> & words, const float * d, int length, std::map<float, int> & results, unsigned int k) const;
virtual std::vector<int> findNN(const std::list<VisualWord *> & vws) const;
void addWordRef(int wordId, int signatureId);
void removeAllWordRef(int wordId, int signatureId);
const VisualWord * getWord(int id) const;
VisualWord * getUnusedWord(int id) const;
void setLastWordId(int id) {_lastWordId = id;}
void getCommonWords(unsigned int nbCommonWords, int totalSign, std::list<int> & commonWords) const;
const std::map<int, VisualWord *> & getVisualWords() const {return _visualWords;}
float getMinDist() const {return _minDist;}
bool isMinDistUsed() const {return _minDistUsed;}
void setMinDistUsed(bool used) {_minDistUsed = used;}
void setNndrUsed(bool used) {_nndrUsed = used;}
bool isNndrUsed() const {return _nndrUsed;}
float getNndrRatio() {return _nndrRatio;}
unsigned int getNotIndexedWordsCount() const {return (int)_notIndexedWords.size();}
int getLastIndexedWordId() const;
int getTotalActiveReferences() const {return _totalActiveReferences;}
void setNNStrategy(NNStrategy strategy, const ParametersMap & parameters = ParametersMap());
NNStrategy nnStrategy() const;
void setNNStrategy(NNStrategy strategy);
bool isIncremental() const {return _incrementalDictionary;}
void setIncrementalDictionary(bool incrementalDictionary, const std::string & dictionaryPath);
void setIncrementalDictionary();
void setFixedDictionary(const std::string & dictionaryPath);
void exportDictionary(const char * fileNameReferences, const char * fileNameDescriptors) const;
void clear();
std::vector<VisualWord *> getUnusedWords() const;
std::vector<int> getUnusedWordIds() const;
unsigned int getUnusedWordsSize() const {return (int)_unusedWords.size();}
void removeWords(const std::vector<VisualWord*> & words); // caller must delete the words
@@ -95,16 +88,12 @@ protected:
private:
bool _incrementalDictionary;
bool _minDistUsed;
float _minDist; //euclidean distance ^ 2
bool _nndrUsed;
float _nndrRatio;
unsigned int _maxLeafs;
std::string _dictionaryPath; // a pre-computed dictionary (.txt)
int _dim;
int _lastWordId;
FlannNN * _nn;
cv::flann::Index * _flannIndex;
cv::Mat _dataTree;
NNStrategy _strategy;
std::map<int ,int> _mapIndexId;
std::map<int, VisualWord*> _unusedWords; //<id,VisualWord*>, note that these words stay in _visualWords
std::set<int> _notIndexedWords; // Words that are not indexed in the dictionary
corelib/include/rtabmap/core/util3d.h
+10 -1
View File
@@ -11,6 +11,7 @@
#include <opencv2/features2d/features2d.hpp>
#include <list>
#include <string>
#include <set>
#include <rtabmap/core/Link.h>
#include <rtabmap/utilite/UThread.h>
@@ -142,6 +143,13 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP transformPointCloud(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const Transform & transform);
pcl::PointXYZ RTABMAP_EXP transformPoint(
const pcl::PointXYZ & pt,
const Transform & transform);
pcl::PointXYZRGB RTABMAP_EXP transformPoint(
const pcl::PointXYZRGB & pt,
const Transform & transform);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDepth(
const cv::Mat & imageDepth,
float depthConstant,
@@ -273,7 +281,8 @@ void RTABMAP_EXP findCorrespondences(
const std::multimap<int, pcl::PointXYZ> & words2,
pcl::PointCloud<pcl::PointXYZ> & inliers1,
pcl::PointCloud<pcl::PointXYZ> & inliers2,
float maxDepth);
float maxDepth,
std::set<int> * uniqueCorrespondences = 0);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cvMat2Cloud(
const cv::Mat & matrix,
corelib/src/CMakeLists.txt
-1
View File
@@ -22,7 +22,6 @@ SET(SRC_FILES
Parameters.cpp
Signature.cpp
Features2d.cpp
NearestNeighbor.cpp
Transform.cpp
util3d.cpp
corelib/src/DBDriverSqlite3.cpp
+8 -4
View File
@@ -1172,7 +1172,9 @@ void DBDriverSqlite3::loadQuery(VWDictionary * dictionary) const
{
UERROR("Saved buffer size (%d) is not the same as descriptor size (%d)", dRealSize/sizeof(float), descriptorSize);
}
VisualWord * vw = new VisualWord(id, &((const float *)descriptor)[0], descriptorSize, 0);
cv::Mat d(1, descriptorSize, CV_32F);
memcpy(d.data, descriptor, dRealSize);
VisualWord * vw = new VisualWord(id, d);
vw->setSaved(true);
dictionary->addWord(vw);
}
@@ -1244,7 +1246,9 @@ void DBDriverSqlite3::loadWordsQuery(const std::set<int> & wordIds, std::list<Vi
UERROR("Saved buffer size (%d) is not the same as descriptor size (%d)", dRealSize/sizeof(float), descriptorSize);
}
VisualWord * vw = new VisualWord(*iter, &((const float *)descriptor)[0], descriptorSize);
cv::Mat d(1, descriptorSize, CV_32F);
memcpy(d.data, descriptor, dRealSize);
VisualWord * vw = new VisualWord(*iter, d);
if(vw)
{
vw->setSaved(true);
@@ -1739,9 +1743,9 @@ void DBDriverSqlite3::saveQuery(const std::list<VisualWord *> & words) const
{
rc = sqlite3_bind_int(ppStmt, 1, w->id());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_int(ppStmt, 2, w->getDim());
rc = sqlite3_bind_int(ppStmt, 2, w->getDescriptor().cols);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_bind_blob(ppStmt, 3, w->getDescriptor(), w->getDim()*sizeof(float), SQLITE_STATIC);
rc = sqlite3_bind_blob(ppStmt, 3, w->getDescriptor().data, w->getDescriptor().cols*sizeof(float), SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
//execute query
corelib/src/Features2d.cpp
+388 -317
View File
@@ -144,218 +144,7 @@ void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors
}
}
/////////////////////
// KeypointDescriptor
/////////////////////
KeypointDescriptor::KeypointDescriptor(const ParametersMap & parameters)
{
this->parseParameters(parameters);
}
KeypointDescriptor::~KeypointDescriptor()
{
}
void KeypointDescriptor::parseParameters(const ParametersMap & parameters)
{
}
//////////////////////////
//SURFDescriptor
//////////////////////////
SURFDescriptor::SURFDescriptor(const ParametersMap & parameters) :
KeypointDescriptor(parameters),
_hessianThreshold(Parameters::defaultSURFHessianThreshold()),
_nOctaves(Parameters::defaultSURFOctaves()),
_nOctaveLayers(Parameters::defaultSURFOctaveLayers()),
_extended(Parameters::defaultSURFExtended()),
_upright(Parameters::defaultSURFUpright()),
_gpuVersion(Parameters::defaultSURFGpuVersion())
{
this->parseParameters(parameters);
}
SURFDescriptor::~SURFDescriptor()
{
}
void SURFDescriptor::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::kSURFGpuVersion(), _gpuVersion);
KeypointDescriptor::parseParameters(parameters);
}
cv::Mat SURFDescriptor::generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{
ULOGGER_DEBUG("");
cv::Mat descriptors;
if(image.empty())
{
ULOGGER_ERROR("Image is null ?!?");
return descriptors;
}
// SURF support only grayscale images
cv::Mat imageGrayScale;
if(image.channels() != 1 || image.depth() != CV_8U)
{
cv::cvtColor(image, imageGrayScale, CV_BGR2GRAY);
}
cv::Mat img;
if(!imageGrayScale.empty())
{
img = imageGrayScale;
}
else
{
img = image;
}
if(_gpuVersion && cv::gpu::getCudaEnabledDeviceCount())
{
std::vector<float> d;
cv::gpu::GpuMat imgGpu(img);
cv::gpu::GpuMat descriptorsGpu;
cv::gpu::GpuMat keypointsGpu;
cv::gpu::SURF_GPU surfGpu(_hessianThreshold, _nOctaves, _nOctaveLayers, _extended, 0.01f, _upright);
surfGpu.uploadKeypoints(keypoints, keypointsGpu);
surfGpu(imgGpu, cv::gpu::GpuMat(), keypointsGpu, descriptorsGpu, true);
surfGpu.downloadDescriptors(descriptorsGpu, d);
unsigned int dim = _extended?128:64;
descriptors = cv::Mat(d.size()/dim, dim, CV_32F);
for(int i=0; i<descriptors.rows; ++i)
{
float * rowFl = descriptors.ptr<float>(i);
memcpy(rowFl, &d[i*dim], dim*sizeof(float));
}
}
else
{
if(_gpuVersion)
{
UWARN("GPU version of SURF not available! Using CPU version instead...");
}
cv::SURF extractor(_hessianThreshold, _nOctaves, _nOctaveLayers, _extended, _upright);
extractor.compute(img, keypoints, descriptors);
}
return descriptors;
}
//////////////////////////
//SIFTDescriptor
//////////////////////////
SIFTDescriptor::SIFTDescriptor(const ParametersMap & parameters) :
KeypointDescriptor(parameters),
_nfeatures(Parameters::defaultSIFTNFeatures()),
_nOctaveLayers(Parameters::defaultSIFTNOctaveLayers()),
_contrastThreshold(Parameters::defaultSIFTContrastThreshold()),
_edgeThreshold(Parameters::defaultSIFTEdgeThreshold()),
_sigma(Parameters::defaultSIFTSigma())
{
this->parseParameters(parameters);
}
SIFTDescriptor::~SIFTDescriptor()
{
}
void SIFTDescriptor::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
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);
KeypointDescriptor::parseParameters(parameters);
}
cv::Mat SIFTDescriptor::generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{
ULOGGER_DEBUG("");
cv::Mat descriptors;
if(image.empty())
{
ULOGGER_ERROR("Image is null ?!?");
return descriptors;
}
// SURF support only grayscale images
cv::Mat imageGrayScale;
if(image.channels() != 1 || image.depth() != CV_8U)
{
cv::cvtColor(image, imageGrayScale, CV_BGR2GRAY);
}
cv::Mat img;
if(!imageGrayScale.empty())
{
img = imageGrayScale;
}
else
{
img = image;
}
cv::SIFT extractor(_nfeatures, _nOctaveLayers, _contrastThreshold, _edgeThreshold, _sigma);
extractor.compute(img, keypoints, descriptors);
return descriptors;
}
/////////////////////
// KeypointDetector
/////////////////////
KeypointDetector::KeypointDetector(const ParametersMap & parameters)
{
this->parseParameters(parameters);
}
void KeypointDetector::parseParameters(const ParametersMap & parameters)
{
}
std::vector<cv::KeyPoint> KeypointDetector::generateKeypoints(
const cv::Mat & image,
int maxKeypoints,
const cv::Rect & roi)
{
ULOGGER_DEBUG("");
std::vector<cv::KeyPoint> keypoints;
if(!image.empty())
{
UTimer timer;
// Get keypoints
keypoints = this->_generateKeypoints(image, roi.width && roi.height?roi:cv::Rect(0,0,image.cols, image.rows));
ULOGGER_DEBUG("Keypoints extraction time = %f s, keypoints extracted = %d", timer.ticks(), keypoints.size());
limitKeypoints(keypoints, maxKeypoints);
if(roi.x || roi.y)
{
// Adjust keypoint position to raw image
for(std::vector<cv::KeyPoint>::iterator iter=keypoints.begin(); iter!=keypoints.end(); ++iter)
{
iter->pt.x += roi.x;
iter->pt.y += roi.y;
}
}
}
else
{
ULOGGER_ERROR("Image is null!");
}
return keypoints;
}
cv::Rect KeypointDetector::computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios)
cv::Rect computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios)
{
if(!image.empty() && roiRatios.size() == 4)
{
@@ -401,144 +190,426 @@ cv::Rect KeypointDetector::computeRoi(const cv::Mat & image, const std::vector<f
}
}
//////////////////////////
//SURFDetector
//////////////////////////
SURFDetector::SURFDetector(const ParametersMap & parameters) :
KeypointDetector(parameters),
_hessianThreshold(Parameters::defaultSURFHessianThreshold()),
_nOctaves(Parameters::defaultSURFOctaves()),
_nOctaveLayers(Parameters::defaultSURFOctaveLayers()),
_extended(Parameters::defaultSURFExtended()),
_upright(Parameters::defaultSURFUpright()),
_gpuVersion(Parameters::defaultSURFGpuVersion())
{
this->parseParameters(parameters);
}
SURFDetector::~SURFDetector()
{
}
void SURFDetector::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::kSURFGpuVersion(), _gpuVersion);
KeypointDetector::parseParameters(parameters);
}
std::vector<cv::KeyPoint> SURFDetector::_generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const
/////////////////////
// Feature2D
/////////////////////
std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, int maxKeypoints, const cv::Rect & roi) const
{
ULOGGER_DEBUG("");
std::vector<cv::KeyPoint> keypoints;
if(image.empty())
if(!image.empty() && image.channels() == 1 && image.type() == CV_8U)
{
ULOGGER_ERROR("Image is null ?!?");
return keypoints;
UTimer timer;
// Get keypoints
keypoints = this->generateKeypointsImpl(image, roi.width && roi.height?roi:cv::Rect(0,0,image.cols, image.rows));
ULOGGER_DEBUG("Keypoints extraction time = %f s, keypoints extracted = %d", timer.ticks(), keypoints.size());
limitKeypoints(keypoints, maxKeypoints);
if(roi.x || roi.y)
{
// Adjust keypoint position to raw image
for(std::vector<cv::KeyPoint>::iterator iter=keypoints.begin(); iter!=keypoints.end(); ++iter)
{
iter->pt.x += roi.x;
iter->pt.y += roi.y;
}
}
}
// SURF support only grayscale images
cv::Mat imageGrayScale;
if(image.channels() != 1 || image.depth() != CV_8U)
else if(image.empty())
{
ULOGGER_DEBUG("");
cv::cvtColor(image, imageGrayScale, CV_BGR2GRAY);
}
cv::Mat img;
if(!imageGrayScale.empty())
{
img = imageGrayScale;
UERROR("Image is null!");
}
else
{
img = image;
UERROR("Image format must be mono8. Current has %d channels and type = %d, size=%d,%d",
image.channels(), image.type(), image.cols, image.rows);
}
cv::Mat imgRoi(img, roi);
if(_gpuVersion && cv::gpu::getCudaEnabledDeviceCount())
return keypoints;
}
cv::Mat Feature2D::generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{
return generateDescriptorsImpl(image, keypoints);
}
//////////////////////////
//SURF
//////////////////////////
SURF::SURF(const ParametersMap & parameters) :
_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())
{
cv::gpu::GpuMat imgGpu(imgRoi);
cv::gpu::GpuMat keypointsGpu;
cv::gpu::SURF_GPU surfGpu(_hessianThreshold, _nOctaves, _nOctaveLayers, _extended, 0.01f, _upright);
surfGpu(imgGpu, cv::gpu::GpuMat(), keypointsGpu);
surfGpu.downloadKeypoints(keypointsGpu, keypoints);
_gpuSurf = new cv::gpu::SURF_GPU(hessianThreshold, nOctaves, nOctaveLayers, extended, gpuKeypointsRatio, upright);
}
else
{
if(_gpuVersion)
if(gpuVersion)
{
UWARN("GPU version of SURF not available! Using CPU version instead...");
}
ULOGGER_DEBUG("%f %d %d %d %d", _hessianThreshold, _nOctaves, _nOctaveLayers, _extended?1:0, _upright?1:0);
cv::SURF detector(_hessianThreshold, _nOctaves, _nOctaveLayers, _extended, _upright);
detector.detect(imgRoi, keypoints);
_surf = new cv::SURF (hessianThreshold, nOctaves, nOctaveLayers, extended, upright);
}
ULOGGER_DEBUG("");
return keypoints;
}
//////////////////////////
//SIFTDetector
//////////////////////////
SIFTDetector::SIFTDetector(const ParametersMap & parameters) :
KeypointDetector(parameters),
_nfeatures(Parameters::defaultSIFTNFeatures()),
_nOctaveLayers(Parameters::defaultSIFTNOctaveLayers()),
_contrastThreshold(Parameters::defaultSIFTContrastThreshold()),
_edgeThreshold(Parameters::defaultSIFTEdgeThreshold()),
_sigma(Parameters::defaultSIFTSigma())
SURF::~SURF()
{
this->parseParameters(parameters);
if(_surf)
{
delete _surf;
}
if(_gpuSurf)
{
delete _gpuSurf;
}
}
SIFTDetector::~SIFTDetector()
std::vector<cv::KeyPoint> SURF::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi) const
{
}
void SIFTDetector::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);
KeypointDetector::parseParameters(parameters);
}
std::vector<cv::KeyPoint> SIFTDetector::_generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const
{
ULOGGER_DEBUG("");
UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
std::vector<cv::KeyPoint> keypoints;
if(image.empty())
cv::Mat imgRoi(image, roi);
if(_gpuSurf)
{
ULOGGER_ERROR("Image is null ?!?");
return keypoints;
}
// SURF support only grayscale images
cv::Mat imageGrayScale;
if(image.channels() != 1 || image.depth() != CV_8U)
{
cv::cvtColor(image, imageGrayScale, CV_BGR2GRAY);
}
cv::Mat img;
if(!imageGrayScale.empty())
{
img = imageGrayScale;
cv::gpu::GpuMat imgGpu(imgRoi);
(*_gpuSurf)(imgGpu, cv::gpu::GpuMat(), keypoints);
}
else
{
img = image;
_surf->detect(imgRoi, keypoints);
}
cv::Mat imgRoi(img, roi);
cv::SIFT detector(_nfeatures, _nOctaveLayers, _contrastThreshold, _edgeThreshold, _sigma);
detector.detect(imgRoi, keypoints); // Opencv surf keypoints
return keypoints;
}
cv::Mat SURF::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{
UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
cv::Mat descriptors;
if(_gpuSurf)
{
cv::gpu::GpuMat imgGpu(image);
cv::gpu::GpuMat descriptorsGPU;
(*_gpuSurf)(imgGpu, cv::gpu::GpuMat(), keypoints, descriptorsGPU, true);
// Download descriptors
if (descriptorsGPU.empty())
descriptors = cv::Mat();
else
{
UASSERT(descriptorsGPU.type() == CV_32F);
descriptors = cv::Mat(descriptorsGPU.size(), CV_32F);
descriptorsGPU.download(descriptors);
}
}
else
{
_surf->compute(image, keypoints, descriptors);
}
return descriptors;
}
//////////////////////////
//SIFT
//////////////////////////
SIFT::SIFT(const ParametersMap & parameters) :
_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);
}
SIFT::~SIFT()
{
if(_sift)
{
delete _sift;
}
}
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);
std::vector<cv::KeyPoint> keypoints;
cv::Mat imgRoi(image, roi);
_sift->detect(imgRoi, keypoints); // Opencv keypoints
return keypoints;
}
cv::Mat SIFT::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{
UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
cv::Mat descriptors;
_sift->compute(image, keypoints, descriptors);
return descriptors;
}
//////////////////////////
//ORB
//////////////////////////
ORB::ORB(const ParametersMap & parameters) :
_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);
}
}
ORB::~ORB()
{
if(_orb)
{
delete _orb;
}
if(_gpuOrb)
{
delete _gpuOrb;
}
}
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);
std::vector<cv::KeyPoint> keypoints;
cv::Mat imgRoi(image, roi);
if(_gpuOrb)
{
cv::gpu::GpuMat imgGpu(imgRoi);
(*_gpuOrb)(imgGpu, cv::gpu::GpuMat(), keypoints);
}
else
{
_orb->detect(imgRoi, keypoints);
}
return keypoints;
}
cv::Mat ORB::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{
UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
cv::Mat descriptors;
if(image.empty())
{
ULOGGER_ERROR("Image is null ?!?");
return descriptors;
}
if(_gpuOrb)
{
cv::gpu::GpuMat imgGpu(image);
cv::gpu::GpuMat descriptorsGPU;
(*_gpuOrb)(imgGpu, cv::gpu::GpuMat(), keypoints, descriptorsGPU);
// Download descriptors
if (descriptorsGPU.empty())
descriptors = cv::Mat();
else
{
UASSERT(descriptorsGPU.type() == CV_32F);
descriptors = cv::Mat(descriptorsGPU.size(), CV_32F);
descriptorsGPU.download(descriptors);
}
}
else
{
_orb->compute(image, keypoints, descriptors);
}
return descriptors;
}
//////////////////////////
//FAST
//////////////////////////
FAST::FAST(const ParametersMap & parameters) :
_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);
}
}
FAST::~FAST()
{
if(_fast)
{
delete _fast;
}
if(_gpuFast)
{
delete _gpuFast;
}
}
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);
std::vector<cv::KeyPoint> keypoints;
cv::Mat imgRoi(image, roi);
if(_gpuFast)
{
cv::gpu::GpuMat imgGpu(imgRoi);
(*_gpuFast)(imgGpu, cv::gpu::GpuMat(), keypoints);
}
else
{
_fast->detect(imgRoi, keypoints); // Opencv keypoints
}
return keypoints;
}
//////////////////////////
//FAST-BRIEF
//////////////////////////
FAST_BRIEF::FAST_BRIEF(const ParametersMap & parameters) :
FAST(parameters),
_brief(0)
{
int bytes = Parameters::defaultBRIEFBytes();
Parameters::parse(parameters, Parameters::kBRIEFBytes(), bytes);
_brief = new cv::BriefDescriptorExtractor(bytes);
}
FAST_BRIEF::~FAST_BRIEF()
{
if(_brief)
{
delete _brief;
}
}
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);
cv::Mat descriptors;
_brief->compute(image, keypoints, descriptors);
return descriptors;
}
//////////////////////////
//FAST-FREAK
//////////////////////////
FAST_FREAK::FAST_FREAK(const ParametersMap & parameters) :
FAST(parameters),
_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);
}
FAST_FREAK::~FAST_FREAK()
{
if(_freak)
{
delete _freak;
}
}
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);
cv::Mat descriptors;
_freak->compute(image, keypoints, descriptors);
return descriptors;
}
}
corelib/src/Memory.cpp
+88 -73
View File
@@ -63,8 +63,8 @@ Memory::Memory(const ParametersMap & parameters) :
_memoryChanged(false),
_signaturesAdded(0),
_keypointDetector(0),
_keypointDescriptor(0),
_feature2D(0),
_featureType(Feature2D::kFeatureUndef),
_badSignRatio(Parameters::defaultKpBadSignRatio()),
_tfIdfLikelihoodUsed(Parameters::defaultKpTfIdfLikelihoodUsed()),
_parallelized(Parameters::defaultKpParallelized()),
@@ -152,7 +152,7 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
if(!((*iter)->isBadSignature() && _badSignaturesIgnored))
{
_signatures.insert(std::pair<int, Signature *>((*iter)->id(), *iter));
if((int)_stMem.size() <= _maxStMemSize)
if(_maxStMemSize == 0 || (int)_stMem.size() <= _maxStMemSize)
{
_stMem.insert((*iter)->id());
}
@@ -266,13 +266,9 @@ Memory::~Memory()
}
}
if(_keypointDetector)
if(_feature2D)
{
delete _keypointDetector;
}
if(_keypointDescriptor)
{
delete _keypointDescriptor;
delete _feature2D;
}
if(_vwd)
{
@@ -294,7 +290,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kMemRecentWmRatio(), _recentWmRatio);
Parameters::parse(parameters, Parameters::kMemSTMSize(), _maxStMemSize);
UASSERT_MSG(_maxStMemSize > 0, uFormat("value=%d", _maxStMemSize).c_str());
UASSERT_MSG(_maxStMemSize >= 0, uFormat("value=%d", _maxStMemSize).c_str());
UASSERT_MSG(_similarityThreshold >= 0.0f && _similarityThreshold <= 1.0f, uFormat("value=%f", _similarityThreshold).c_str());
UASSERT_MSG(_recentWmRatio >= 0.0f && _recentWmRatio <= 1.0f, uFormat("value=%f", _recentWmRatio).c_str());
@@ -368,48 +364,45 @@ void Memory::parseParameters(const ParametersMap & parameters)
}
//Keypoint detector
KeypointDetector::DetectorType detectorStrategy = KeypointDetector::kDetectorUndef;
Feature2D::Type detectorStrategy = Feature2D::kFeatureUndef;
if((iter=parameters.find(Parameters::kKpDetectorStrategy())) != parameters.end())
{
detectorStrategy = (KeypointDetector::DetectorType)std::atoi((*iter).second.c_str());
detectorStrategy = (Feature2D::Type)std::atoi((*iter).second.c_str());
}
if(!_keypointDetector || detectorStrategy!=KeypointDetector::kDetectorUndef)
if(detectorStrategy!=Feature2D::kFeatureUndef)
{
UDEBUG("new detector strategy %d", int(detectorStrategy));
if(_keypointDetector)
if(_feature2D)
{
delete _keypointDetector;
_keypointDetector = 0;
}
if(_keypointDescriptor)
{
delete _keypointDescriptor;
_keypointDescriptor = 0;
delete _feature2D;
_feature2D = 0;
_featureType = Feature2D::kFeatureUndef;
}
switch(detectorStrategy)
{
case KeypointDetector::kDetectorSift:
_keypointDetector = new SIFTDetector(parameters);
_keypointDescriptor = new SIFTDescriptor(parameters);
case Feature2D::kFeatureSift:
_feature2D = new SIFT(parameters);
_featureType = Feature2D::kFeatureSift;
break;
case KeypointDetector::kDetectorSurf:
case Feature2D::kFeatureFastBrief:
_feature2D = new FAST_BRIEF(parameters);
_featureType = Feature2D::kFeatureFastBrief;
break;
case Feature2D::kFeatureFastFreak:
_feature2D = new FAST_FREAK(parameters);
_featureType = Feature2D::kFeatureFastFreak;
break;
case Feature2D::kFeatureOrb:
_feature2D = new ORB(parameters);
_featureType = Feature2D::kFeatureOrb;
break;
case Feature2D::kFeatureSurf:
default:
_keypointDetector = new SURFDetector(parameters);
_keypointDescriptor = new SURFDescriptor(parameters);
_feature2D = new SURF(parameters);
_featureType = Feature2D::kFeatureSurf;
break;
}
}
else
{
if(_keypointDetector)
{
_keypointDetector->parseParameters(parameters);
}
if(_keypointDescriptor)
{
_keypointDescriptor->parseParameters(parameters);
}
}
}
void Memory::preUpdate()
@@ -487,7 +480,7 @@ bool Memory::update(const Image & image, Statistics * stats)
//============================================================
// Transfer the oldest signature of the short-term memory to the working memory
//============================================================
while(_stMem.size() && (int)_stMem.size() > _maxStMemSize)
while(_stMem.size() && _maxStMemSize>0 && (int)_stMem.size() > _maxStMemSize)
{
UDEBUG("Inserting node %d from STM in WM...", *_stMem.begin());
_workingMem.insert(_workingMem.end(), *_stMem.begin());
@@ -616,9 +609,9 @@ Signature * Memory::_getSignature(int id) const
return uValue(_signatures, id, (Signature*)0);
}
int Memory::getVWDictionarySize() const
const VWDictionary * Memory::getVWDictionary() const
{
return _vwd->getVisualWords().size();
return _vwd;
}
void Memory::getPose(int locationId, Transform & pose, bool lookInDatabase) const
@@ -1436,7 +1429,10 @@ std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<
return removableSignatures;
}
void Memory::moveToTrash(Signature * s, bool saveToDatabase)
/**
* If saveToDatabase=false, deleted words are filled in deletedWords.
*/
void Memory::moveToTrash(Signature * s, bool saveToDatabase, std::list<int> * deletedWords)
{
UDEBUG("id=%d", s?s->id():0);
if(s)
@@ -1453,9 +1449,10 @@ void Memory::moveToTrash(Signature * s, bool saveToDatabase)
// neighbor to s
if(n)
{
if(iter->first > s->id() && (n->getNeighbors().size() > 1 || !n->hasNeighbor(s->id())))
if(iter->first > s->id() && (n->getNeighbors().size() > 2 || !n->hasNeighbor(s->id())))
{
UWARN("Neighbor %d of %d is newer, removing neighbor link may split the map!", iter->first, s->id());
UWARN("Neighbor %d of %d is newer, removing neighbor link may split the map!",
iter->first, s->id());
}
n->removeNeighbor(s->id());
@@ -1503,7 +1500,27 @@ void Memory::moveToTrash(Signature * s, bool saveToDatabase)
s->setWeight(0);
}
this->disableWordsRef(s->id(), saveToDatabase);
this->disableWordsRef(s->id());
if(!saveToDatabase)
{
std::list<int> keys = uUniqueKeys(s->getWords());
for(std::list<int>::const_iterator i=keys.begin(); i!=keys.end(); ++i)
{
// assume just removed word doesn't have any other references
VisualWord * w = _vwd->getUnusedWord(*i);
if(w)
{
std::vector<VisualWord*> wordToDelete;
wordToDelete.push_back(w);
_vwd->removeWords(wordToDelete);
if(deletedWords)
{
deletedWords->push_back(w->id());
}
delete w;
}
}
}
_workingMem.erase(s->id());
_stMem.erase(s->id());
@@ -1542,13 +1559,13 @@ const Signature * Memory::getLastWorkingSignature() const
return _lastSignature;
}
void Memory::deleteLocation(int locationId)
void Memory::deleteLocation(int locationId, std::list<int> * deletedWords)
{
UINFO("Deleting location %d", locationId);
UDEBUG("Deleting location %d", locationId);
Signature * location = _getSignature(locationId);
if(location)
{
this->moveToTrash(location, false);
this->moveToTrash(location, false, deletedWords);
}
}
@@ -1640,12 +1657,12 @@ Transform Memory::computeVisualTransform(const Signature & oldS, const Signature
}
else if(inliersCount < _bowMinInliers)
{
UINFO("Not enough inliers %d/%d between %d and %d", inliersCount, _bowMinInliers, oldS.id(), newS.id());
UINFO("Not enough inliers (after RANSAC) %d/%d between %d and %d", inliersCount, _bowMinInliers, oldS.id(), newS.id());
}
}
else
{
UDEBUG("Not enough inliers %d/%d between %d and %d", (int)inliersOld->size(), _bowMinInliers, oldS.id(), newS.id());
UINFO("Not enough inliers %d/%d between %d and %d", (int)inliersOld->size(), _bowMinInliers, oldS.id(), newS.id());
}
}
else if(!oldS.isBadSignature() && !newS.isBadSignature())
@@ -2783,10 +2800,10 @@ void Memory::extractKeypointsAndDescriptors(
if(_wordsPerImageTarget >= 0)
{
UTimer timer;
if(_keypointDetector)
if(_feature2D)
{
cv::Rect roi = KeypointDetector::computeRoi(image, _roiRatios);
keypoints = _keypointDetector->generateKeypoints(image, 0, roi);
cv::Rect roi = computeRoi(image, _roiRatios);
keypoints = _feature2D->generateKeypoints(image, 0, roi);
UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), timer.ticks());
filterKeypointsByDepth(keypoints, depth, depthConstant, _wordsMaxDepth);
@@ -2795,7 +2812,7 @@ void Memory::extractKeypointsAndDescriptors(
if(keypoints.size())
{
descriptors = _keypointDescriptor->generateDescriptors(image, keypoints);
descriptors = _feature2D->generateDescriptors(image, keypoints);
UDEBUG("time descriptors (%d) = %fs", descriptors.rows, timer.ticks());
}
}
@@ -2876,12 +2893,12 @@ Signature * Memory::createSignature(const Image & image, bool keepRawData)
preUpdateThread.start();
}
if(!image.descriptors().empty())
if(!image.descriptors().empty() && image.featureType() == _featureType)
{
// DESCRIPTORS
if(image.descriptors().rows && image.descriptors().rows >= _badSignRatio * float(meanWordsPerLocation))
{
UASSERT(image.descriptors().type() == CV_32F);
UASSERT(image.descriptors().type() == CV_32F || image.descriptors().type() == CV_8U);
descriptors = image.descriptors();
keypoints = image.keypoints();
}
@@ -2891,7 +2908,18 @@ Signature * Memory::createSignature(const Image & image, bool keepRawData)
else
{
// IMAGE RAW
this->extractKeypointsAndDescriptors(image.image(), image.depth(), image.depthConstant(), keypoints, descriptors);
cv::Mat imageMono;
// convert to grayscale
if(image.image().channels() > 1)
{
cv::cvtColor(image.image(), imageMono, cv::COLOR_BGR2GRAY);
}
else
{
imageMono = image.image();
}
this->extractKeypointsAndDescriptors(imageMono, image.depth(), image.depthConstant(), keypoints, descriptors);
UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
@@ -2999,11 +3027,11 @@ Signature * Memory::createSignature(const Image & image, bool keepRawData)
return s;
}
void Memory::disableWordsRef(int signatureId, bool saveToDatabase)
void Memory::disableWordsRef(int signatureId)
{
UDEBUG("id=%d", signatureId);
Signature * ss = dynamic_cast<Signature *>(this->_getSignature(signatureId));
Signature * ss = this->_getSignature(signatureId);
if(ss && ss->isEnabled())
{
const std::multimap<int, cv::KeyPoint> & words = ss->getWords();
@@ -3013,18 +3041,6 @@ void Memory::disableWordsRef(int signatureId, bool saveToDatabase)
for(std::list<int>::const_iterator i=keys.begin(); i!=keys.end(); ++i)
{
_vwd->removeAllWordRef(*i, signatureId);
if(!saveToDatabase)
{
// assume just removed word doesn't have any other references
VisualWord * w = _vwd->getUnusedWord(*i);
if(w)
{
std::vector<VisualWord*> wordToDelete;
wordToDelete.push_back(w);
_vwd->removeWords(wordToDelete);
delete w;
}
}
}
count -= _vwd->getTotalActiveReferences();
@@ -3103,8 +3119,7 @@ void Memory::enableWordsRef(const std::list<int> & signatureIds)
if(vws.size())
{
//Search in the dictionary
bool reactivatedWordsComparedToNewWords = true;
std::vector<int> vwActiveIds = _vwd->findNN(vws, reactivatedWordsComparedToNewWords);
std::vector<int> vwActiveIds = _vwd->findNN(vws);
UDEBUG("find active ids (number=%d) time=%fs", vws.size(), timer.ticks());
int i=0;
for(std::list<VisualWord *>::iterator iterVws=vws.begin(); iterVws!=vws.end(); ++iterVws)
corelib/src/NearestNeighbor.cpp
-111
View File
@@ -1,111 +0,0 @@
/*
* Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
*
* This file is part of RTAB-Map.
*
* RTAB-Map is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RTAB-Map is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#include "NearestNeighbor.h"
#include "rtabmap/utilite/ULogger.h"
#include <opencv2/core/core.hpp>
namespace rtabmap
{
/////////////////////////
// FlannNN
/////////////////////////
FlannNN::FlannNN(Strategy strategy, const ParametersMap & parameters) :
_treeFlannIndex(0),
_strategy(strategy)
{
ULOGGER_DEBUG("");
this->parseParameters(parameters);
}
FlannNN::~FlannNN() {
if(_treeFlannIndex)
{
delete _treeFlannIndex;
}
}
void FlannNN::setData(const cv::Mat & data)
{
if(_treeFlannIndex)
{
delete _treeFlannIndex;
_treeFlannIndex = 0;
}
_treeFlannIndex = createIndex(data, _strategy); // using 4 randomized trees
}
void FlannNN::search(const cv::Mat & queries, cv::Mat & indices, cv::Mat & dists, int knn, int emax)
{
ULOGGER_DEBUG("");
if(_treeFlannIndex)
{
// Note, the search params is ignored because we use an autotuned created index (see update())
_treeFlannIndex->knnSearch(queries, indices, dists, knn, cv::flann::SearchParams(emax) ); // maximum number of leafs checked
}
else
{
ULOGGER_ERROR("The search index is not created, setData() must be called first");
}
}
void FlannNN::search(const cv::Mat & data, const cv::Mat & queries, cv::Mat & indices, cv::Mat & dists, int knn, int emax) const
{
ULOGGER_DEBUG("");
cv::flann::Index * index = createIndex(data, _strategy);
// Note, the search params is ignored because we use an autotuned created index (see update())
index->knnSearch(queries, indices, dists, knn, cv::flann::SearchParams(emax) ); // maximum number of leafs checked
delete index;
}
void FlannNN::parseParameters(const ParametersMap & parameters)
{
}
enum Strategy{kLinear, kKDTree, kMeans, kComposite, kAutoTuned, kUndefined};
cv::flann::Index * FlannNN::createIndex(const cv::Mat & data, Strategy s) const
{
cv::flann::Index * index = 0;
switch(s)
{
case kLinear:
index = new cv::flann::Index(data, cv::flann::LinearIndexParams());
break;
case kKDTree:
index = new cv::flann::Index(data, cv::flann::KDTreeIndexParams());
break;
case kMeans:
index = new cv::flann::Index(data, cv::flann::KMeansIndexParams());
break;
case kComposite:
index = new cv::flann::Index(data, cv::flann::CompositeIndexParams());
break;
case kAutoTuned:
default:
index = new cv::flann::Index(data, cv::flann::AutotunedIndexParams());
break;
}
return index;
}
}
corelib/src/NearestNeighbor.h
-76
View File
@@ -1,76 +0,0 @@
/*
* Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
*
* This file is part of RTAB-Map.
*
* RTAB-Map is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RTAB-Map is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef NEARESTNEIGHBOR_H_
#define NEARESTNEIGHBOR_H_
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/imgproc/imgproc_c.h>
#include <map>
#include "rtabmap/core/Parameters.h"
namespace rtabmap
{
/////////////////////////
// FlannNN
/////////////////////////
class RTABMAP_EXP FlannNN
{
public:
enum dummy {d}; // Hack, to fix Eclipse complaining about not defined Strategy enum ?!
enum Strategy{kLinear, kKDTree, kMeans, kComposite, kAutoTuned, kUndefined};
public:
FlannNN(Strategy s = kKDTree, const ParametersMap & parameters = ParametersMap());
virtual ~FlannNN();
void setStrategy(Strategy s) {if(_strategy!=kUndefined) _strategy = s;}
virtual void setData(const cv::Mat & data);
virtual void search(const cv::Mat & queries,
cv::Mat & indices,
cv::Mat & dists,
int knn = 1,
int emax = 64);
virtual void search(const cv::Mat & data,
const cv::Mat & queries,
cv::Mat & indices,
cv::Mat & dists,
int knn = 1,
int emax = 64) const;
virtual void parseParameters(const ParametersMap & parameters);
private:
cv::flann::Index * createIndex(const cv::Mat & data, Strategy s) const;
private:
cv::flann::Index * _treeFlannIndex;
Strategy _strategy;
};
}
#endif /* NEARESTNEIGHBOR_H_ */
corelib/src/Odometry.cpp
+162 -369
View File
@@ -18,11 +18,14 @@
#include <rtabmap/core/Features2d.h>
#include <rtabmap/core/Memory.h>
#include <rtabmap/core/VWDictionary.h>
#include "rtabmap/core/Signature.h"
#include <pcl/io/pcd_io.h>
#include <pcl/common/transforms.h>
#include <opencv2/gpu/gpu.hpp>
#if _MSC_VER
#define ISFINITE(value) _finite(value)
#else
@@ -31,31 +34,6 @@
namespace rtabmap {
Odometry::Odometry(
float inlierDistance,
int maxWords,
int minInliers,
int iterations,
float wordsRatio,
float maxDepth,
float linearUpdate,
float angularUpdate,
int resetCoutdown) :
_maxFeatures(maxWords),
_minInliers(minInliers),
_inlierDistance(inlierDistance),
_iterations(iterations),
_wordsRatio(wordsRatio),
_maxDepth(maxDepth),
_linearUpdate(linearUpdate),
_angularUpdate(angularUpdate),
_resetCountdown(resetCoutdown),
_pose(Transform::getIdentity()),
_resetCurrentCount(0)
{
}
Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
_maxFeatures(Parameters::defaultOdomMaxWords()),
_minInliers(Parameters::defaultOdomMinInliers()),
@@ -66,6 +44,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
_linearUpdate(Parameters::defaultOdomLinearUpdate()),
_angularUpdate(Parameters::defaultOdomAngularUpdate()),
_resetCountdown(Parameters::defaultOdomResetCountdown()),
_localHistory(Parameters::defaultOdomLocalHistory()),
_pose(Transform::getIdentity()),
_resetCurrentCount(0)
{
@@ -78,6 +57,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
Parameters::parse(parameters, Parameters::kOdomWordsRatio(), _wordsRatio);
Parameters::parse(parameters, Parameters::kOdomMaxDepth(), _maxDepth);
Parameters::parse(parameters, Parameters::kOdomMaxWords(), _maxFeatures);
Parameters::parse(parameters, Parameters::kOdomLocalHistory(), _localHistory);
}
void Odometry::reset()
@@ -116,57 +96,71 @@ Transform Odometry::process(Image & image, int * quality)
}
return Transform();
}
OdometryBinary::OdometryBinary(
float inlierDistance,
int maxWords,
int minInliers,
int iterations,
float wordsRatio,
float maxDepth,
float linearUpdate,
float angularUpdate,
int resetCoutdown,
int briefBytes,
int fastThreshold,
bool fastNonmaxSuppression,
bool bruteForceMatching) :
Odometry(inlierDistance, maxWords, minInliers, iterations, wordsRatio, maxDepth, linearUpdate, angularUpdate, resetCoutdown),
_briefBytes(briefBytes),
_fastThreshold(fastThreshold),
_fastNonmaxSuppression(fastNonmaxSuppression),
_bruteForceMatching(bruteForceMatching)
{
}
OdometryBinary::OdometryBinary(const ParametersMap & parameters) :
//OdometryBOW
OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
Odometry(parameters),
_briefBytes(Parameters::defaultOdomBinBriefBytes()),
_fastThreshold(Parameters::defaultOdomBinFastThreshold()),
_fastNonmaxSuppression(Parameters::defaultOdomBinFastNonmaxSuppression()),
_bruteForceMatching(Parameters::defaultOdomBinBruteForceMatching())
_memory(0)
{
Parameters::parse(parameters, Parameters::kOdomBinBriefBytes(), _briefBytes);
Parameters::parse(parameters, Parameters::kOdomBinFastThreshold(), _fastThreshold);
Parameters::parse(parameters, Parameters::kOdomBinFastNonmaxSuppression(), _fastNonmaxSuppression);
Parameters::parse(parameters, Parameters::kOdomBinBruteForceMatching(), _bruteForceMatching);
ParametersMap customParameters;
customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(this->getMaxFeatures()))); // hack
customParameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(this->getMaxDepth())));
customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
customParameters.insert(ParametersPair(Parameters::kMemImageKept(), "false"));
customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
int nn = Parameters::defaultOdomNearestNeighbor();
float nndr = Parameters::defaultOdomNNDR();
int odomType = Parameters::defaultOdomType();
Parameters::parse(parameters, Parameters::kOdomNearestNeighbor(), nn);
Parameters::parse(parameters, Parameters::kOdomNNDR(), nndr);
Parameters::parse(parameters, Parameters::kOdomType(), odomType);
customParameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(nn)));
customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(nndr)));
customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(odomType)));
// add only feature stuff
for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
{
std::string group = uSplit(iter->first, '/').front();
if(group.compare("SURF") == 0 ||
group.compare("SIFT") == 0 ||
group.compare("BRIEF") == 0 ||
group.compare("FAST") == 0 ||
group.compare("ORB") == 0 ||
group.compare("FREAK") == 0)
{
customParameters.insert(*iter);
}
}
_memory = new Memory(customParameters);
if(!_memory->init("", false, ParametersMap(), false))
{
UERROR("Error initializing the memory for BOW Odometry.");
}
}
void OdometryBinary::reset()
OdometryBOW::~OdometryBOW()
{
delete _memory;
}
void OdometryBOW::reset()
{
Odometry::reset();
_lastKeypoints.clear();
_lastDescriptors = cv::Mat();
_lastDepth = cv::Mat();
_memory->init("", false, ParametersMap(), false);
localMap_.clear();
}
// return not null transform if odometry is correctly computed
Transform OdometryBinary::computeTransform(Image & image, int * quality)
Transform OdometryBOW::computeTransform(Image & image, int * quality)
{
UTimer timer;
cv::Mat imageMono;
Transform output;
cv::Mat imageMono;
// convert to grayscale
if(image.image().channels() > 1)
{
@@ -177,274 +171,19 @@ Transform OdometryBinary::computeTransform(Image & image, int * quality)
imageMono = image.image();
}
cv::FastFeatureDetector detector(_fastThreshold, _fastNonmaxSuppression);
std::vector<cv::KeyPoint> newKeypoints;
detector.detect(imageMono, newKeypoints);
limitKeypoints(newKeypoints, this->getMaxFeatures());
cv::BriefDescriptorExtractor extractor(_briefBytes);
cv::Mat newDescriptors;
extractor.compute(imageMono, newKeypoints, newDescriptors);
int inliers = 0;
int correspondences = 0;
if(_lastKeypoints.size())
{
if(newDescriptors.rows && newDescriptors.rows > (int)(getWordsRatio() * float(_lastKeypoints.size()))) // at least 50% keypoints
{
cv::Mat results;
cv::Mat dists;
int k=1; // find the 1 nearest neighbor
std::vector<std::vector<cv::DMatch> > matches;
if(_bruteForceMatching)
{
cv::BFMatcher matcher(cv::NORM_HAMMING);
matcher.knnMatch(newDescriptors, _lastDescriptors, matches, k);
}
else
{
// Create Flann LSH index
cv::flann::Index flannIndex(_lastDescriptors, cv::flann::LshIndexParams(12, 20, 2), cvflann::FLANN_DIST_HAMMING);
results = cv::Mat(newDescriptors.rows, k, CV_32SC1);
dists = cv::Mat(newDescriptors.rows, k, CV_32FC1);
// search (nearest neighbor)
flannIndex.knnSearch(newDescriptors, results, dists, k, cv::flann::SearchParams() );
}
pcl::PointCloud<pcl::PointXYZ>::Ptr mpts_1(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr mpts_2(new pcl::PointCloud<pcl::PointXYZ>);
std::vector<int> indexes_1, indexes_2;
std::vector<uchar> outlier_mask;
// Check if this descriptor matches with those of the objects
mpts_1->resize(newDescriptors.rows);
mpts_2->resize(newDescriptors.rows);
UDEBUG("newDescriptors=%d _lastKeypoints=%d time=%fs", newDescriptors.rows, _lastKeypoints.size(), timer.elapsed());
int oi = 0;
if(_bruteForceMatching)
{
for(unsigned int i=0; i<matches.size(); ++i)
{
pcl::PointXYZ pt1 = util3d::getDepth(image.depth(),
int(newKeypoints.at(matches.at(i).at(0).queryIdx).pt.x+0.5f),
int(newKeypoints.at(matches.at(i).at(0).queryIdx).pt.y+0.5f),
(float)imageMono.cols/2,
(float)imageMono.rows/2,
1.0f/image.depthConstant(),
1.0f/image.depthConstant());
if(matches.at(i).at(0).trainIdx >=0)
{
pcl::PointXYZ pt2 = util3d::getDepth(_lastDepth,
int(_lastKeypoints.at(matches.at(i).at(0).trainIdx).pt.x+0.5f),
int(_lastKeypoints.at(matches.at(i).at(0).trainIdx).pt.y+0.5f),
(float)imageMono.cols/2,
(float)imageMono.rows/2,
1.0f/image.depthConstant(),
1.0f/image.depthConstant());
if(uIsFinite(pt1.z) && uIsFinite(pt2.z) &&
(this->getMaxDepth() <= 0 || (pt1.z < this->getMaxDepth() && pt2.z < this->getMaxDepth())))
{
mpts_1->at(oi) = pt1;
mpts_2->at(oi) = pt2;
++oi;
}
}
else
{
UWARN("Index = %d for i=%d ?!?", results.at<int>(i,0), i);
}
}
}
else
{
for(int i=0; i<newDescriptors.rows; ++i)
{
pcl::PointXYZ pt1 = util3d::getDepth(image.depth(),
int(newKeypoints.at(i).pt.x+0.5f),
int(newKeypoints.at(i).pt.y+0.5f),
(float)imageMono.cols/2,
(float)imageMono.rows/2,
1.0f/image.depthConstant(),
1.0f/image.depthConstant());
if(results.at<int>(i,0) >=0)
{
pcl::PointXYZ pt2 = util3d::getDepth(_lastDepth,
int(_lastKeypoints.at(results.at<int>(i,0)).pt.x+0.5f),
int(_lastKeypoints.at(results.at<int>(i,0)).pt.y+0.5f),
(float)imageMono.cols/2,
(float)imageMono.rows/2,
1.0f/image.depthConstant(),
1.0f/image.depthConstant());
if(uIsFinite(pt1.z) && uIsFinite(pt2.z) &&
(this->getMaxDepth() <= 0 || (pt1.z < this->getMaxDepth() && pt2.z < this->getMaxDepth())))
{
mpts_1->at(oi) = pt1;
mpts_2->at(oi) = pt2;
++oi;
}
}
else
{
UWARN("Index = %d for i=%d ?!?", results.at<int>(i,0), i);
}
}
}
mpts_1->resize(oi);
mpts_2->resize(oi);
UDEBUG("Correspondences = %d", oi);
if(oi >= this->getMinInliers())
{
mpts_1 = util3d::transformPointCloud(mpts_1, image.localTransform()); // new
mpts_2 = util3d::transformPointCloud(mpts_2, image.localTransform()); // previous
correspondences = mpts_2->size();
Transform t = util3d::transformFromXYZCorrespondences(
mpts_1,
mpts_2,
this->getInlierDistance(),
this->getIterations(),
&inliers);
float x,y,z, roll,pitch,yaw;
pcl::getTranslationAndEulerAngles(util3d::transformToEigen3f(t), x,y,z, roll,pitch,yaw);
if(quality)
{
*quality = inliers;
}
// Large transforms may be erroneous computed transforms, so keep under 1 m
if(inliers >= this->getMinInliers())
{
if(isLargeEnoughTransform(t))
{
_lastKeypoints = newKeypoints;
_lastDescriptors = newDescriptors;
_lastDepth = image.depth().clone();
output = t;
}
else
{
output.setIdentity();
}
}
else
{
UWARN("Transform not valid (inliers = %d/%d)", inliers, correspondences);
}
}
else
{
UWARN("Not enough inliers %d < %d", oi, this->getMinInliers());
}
}
else if(newDescriptors.rows)
{
UWARN("At least %f%% keypoints of the last image required. New=%d last=%d",
getWordsRatio()*100.0f, newDescriptors.rows, _lastKeypoints.size());
}
else
{
UWARN("No feature extracted!");
}
}
else
{
_lastKeypoints = newKeypoints;
_lastDescriptors = newDescriptors;
_lastDepth = image.depth().clone();
output.setIdentity();
}
UINFO("Odom update time = %fs features=%d inliers=%d/%d",
timer.elapsed(),
newDescriptors.rows,
inliers,
correspondences);
return output;
}
//OdometryBOW
OdometryBOW::OdometryBOW(
int detectorType, // SURF or SIFT
float inlierDistance,
int maxWords,
int minInliers,
int iterations,
float wordsRatio,
float maxDepth,
float linearUpdate,
float angularUpdate,
int resetCoutdown,
float surfHessianThreshold,
float nndr) : // nearest neighbor distance ratio
Odometry(inlierDistance, maxWords, minInliers, iterations, wordsRatio, maxDepth, linearUpdate, angularUpdate, resetCoutdown),
_memory(new Memory())
{
ParametersMap customParameters;
customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(maxWords)));
customParameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(maxDepth)));
customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(detectorType)));
customParameters.insert(ParametersPair(Parameters::kSURFHessianThreshold(), uNumber2Str(surfHessianThreshold)));
customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(nndr)));
customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
customParameters.insert(ParametersPair(Parameters::kMemImageKept(), "false"));
if(!_memory->init("", false, customParameters, false))
{
UERROR("Error initializing the memory for BOW Odometry.");
}
}
OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
Odometry(parameters),
_memory(new Memory(parameters))
{
ParametersMap customParameters;
customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(this->getMaxFeatures()))); // hack
customParameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(this->getMaxDepth())));
customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
customParameters.insert(ParametersPair(Parameters::kMemImageKept(), "false"));
if(!_memory->init("", false, customParameters, false))
{
UERROR("Error initializing the memory for BOW Odometry.");
}
}
OdometryBOW::~OdometryBOW()
{
UDEBUG("");
delete _memory;
UDEBUG("");
}
void OdometryBOW::reset()
{
Odometry::reset();
_memory->init("", false, ParametersMap(), false);
}
// return not null transform if odometry is correctly computed
Transform OdometryBOW::computeTransform(Image & image, int * quality)
{
UTimer timer;
Transform output;
std::vector<cv::KeyPoint> keypoints;
cv::Mat descriptors;
_memory->extractKeypointsAndDescriptors(image.image(), image.depth(), image.depthConstant(), keypoints, descriptors);
_memory->extractKeypointsAndDescriptors(imageMono, image.depth(), image.depthConstant(), keypoints, descriptors);
image.setDescriptors(descriptors);
image.setDescriptors(descriptors, _memory->getFeatureType());
image.setKeypoints(keypoints);
if(this->getLocalHistory() && this->getLocalHistory() < descriptors.rows)
{
UWARN("Local history words size (%d) is smaller than extracted features from the current frame (%d).",
this->getLocalHistory(), descriptors.rows);
}
int inliers = 0;
int correspondences = 0;
@@ -455,22 +194,24 @@ Transform OdometryBOW::computeTransform(Image & image, int * quality)
if(previousSignature && newSignature)
{
Transform transform;
std::set<int> uniqueCorrespondences;
if(newSignature->getWords3().size() < (unsigned int)(getWordsRatio() * float(previousSignature->getWords3().size())))
{
UWARN("At least %f%% keypoints of the last image required. New=%d last=%d",
getWordsRatio()*100.0f, newSignature->getWords3().size(), previousSignature->getWords3().size());
}
else if(!previousSignature->getWords3().empty() && !newSignature->getWords3().empty())
else 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
util3d::findCorrespondences(
previousSignature->getWords3(),
localMap_,
newSignature->getWords3(),
*inliers1,
*inliers2,
this->getMaxDepth());
this->getMaxDepth(),
&uniqueCorrespondences);
if((int)inliers1->size() >= this->getMinInliers())
{
@@ -493,10 +234,6 @@ Transform OdometryBOW::computeTransform(Image & image, int * quality)
transform.setNull();
UWARN("Transform not valid (inliers = %d/%d)", inliers, correspondences);
}
//else if(!transform.isNull() && true)
//{
// transform = _memory->computeIcpTransform(*newSignature, *previousSignature, transform, true);
//}
}
else
{
@@ -508,46 +245,121 @@ Transform OdometryBOW::computeTransform(Image & image, int * quality)
{
_memory->deleteLocation(newSignature->id());
}
else if(!isLargeEnoughTransform(transform))
{
output.setIdentity();
_memory->deleteLocation(newSignature->id());
}
else
{
output = transform;
_memory->deleteLocation(previousSignature->id());
if(this->getLocalHistory()<=0)
{
output = this->getPose().inverse() * transform; // make it incremental
if(!isLargeEnoughTransform(transform))
{
// Transform not large enough, keep the old signature
_memory->deleteLocation(newSignature->id());
}
else
{
_memory->deleteLocation(previousSignature->id());
localMap_.clear();
// update local map
std::list<int> uniques = uUniqueKeys(newSignature->getWords3());
for(std::list<int>::iterator iter = uniques.begin(); iter!=uniques.end(); ++iter)
{
if(newSignature->getWords3().count(*iter) == 1)
{
const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second;
if(pcl::isFinite(pt) &&
(pt.x != 0 || pt.y != 0 || pt.z != 0) &&
(this->getMaxDepth() <= 0 || (pt.x <= this->getMaxDepth())))
{
pcl::PointXYZ pt2 = util3d::transformPoint(pt, transform);
localMap_.insert(std::make_pair<int, pcl::PointXYZ>(*iter, pt2));
}
}
}
}
}
else
{
output = this->getPose().inverse() * transform; // make it incremental
if(isLargeEnoughTransform(transform))
{
// update local map only if transform is large enough
std::list<int> uniques = uUniqueKeys(newSignature->getWords3());
for(std::list<int>::iterator iter = uniques.begin(); iter!=uniques.end(); ++iter)
{
if(newSignature->getWords3().count(*iter) == 1 &&
uniqueCorrespondences.find(*iter) == uniqueCorrespondences.end())
{
const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second;
if(pcl::isFinite(pt) &&
(pt.x != 0 || pt.y != 0 || pt.z != 0) &&
(this->getMaxDepth() <= 0 || (pt.x <= this->getMaxDepth())))
{
pcl::PointXYZ pt2 = util3d::transformPoint(pt, transform);
localMap_.insert(std::make_pair<int, pcl::PointXYZ>(*iter, pt2));
}
}
}
while(localMap_.size() && (int)localMap_.size() > this->getLocalHistory() && _memory->getStMem().size()>1)
{
std::list<int> deletedWords;
_memory->deleteLocation(*_memory->getStMem().begin(), &deletedWords);
for(std::list<int>::iterator iter = deletedWords.begin(); iter!=deletedWords.end(); ++iter)
{
localMap_.erase(*iter);
}
}
}
else
{
_memory->deleteLocation(newSignature->id());
}
}
}
}
else if(!previousSignature && newSignature)
{
localMap_.clear();
output.setIdentity();
std::list<int> uniques = uUniqueKeys(newSignature->getWords3());
for(std::list<int>::iterator iter = uniques.begin(); iter!=uniques.end(); ++iter)
{
if(newSignature->getWords3().count(*iter) == 1)
{
const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second;
if(pcl::isFinite(pt) &&
(pt.x != 0 || pt.y != 0 || pt.z != 0) &&
(this->getMaxDepth() <= 0 || (pt.x <= this->getMaxDepth())))
{
localMap_.insert(std::make_pair<int, pcl::PointXYZ>(*iter, pt));
}
}
}
}
_memory->emptyTrash();
}
UINFO("Odom update time = %fs features=%d inliers=%d/%d",
UINFO("Odom update time = %fs features=%d inliers=%d/%d dict=%d nodes=%d",
timer.elapsed(),
descriptors.rows,
inliers,
correspondences);
correspondences,
(int)_memory->getVWDictionary()->getVisualWords().size(),
(int)_memory->getStMem().size());
return output;
}
// OdometryICP
OdometryICP::OdometryICP(
int decimation,
OdometryICP::OdometryICP(int decimation,
float voxelSize,
float samples,
int samples,
float maxCorrespondenceDistance,
int maxIterations,
int maxIterations,
float maxFitness,
float maxDepth,
float linearUpdate,
float angularUpdate,
int resetCoutdown) :
Odometry(0, 0, 0, 0, 0, maxDepth, linearUpdate, angularUpdate, resetCoutdown),
const ParametersMap & odometryParameter) :
Odometry(odometryParameter),
_decimation(decimation),
_voxelSize(voxelSize),
_samples(samples),
@@ -556,25 +368,6 @@ OdometryICP::OdometryICP(
_maxFitness(maxFitness),
_previousCloud(new pcl::PointCloud<pcl::PointNormal>)
{
}
OdometryICP::OdometryICP(const ParametersMap & parameters) :
Odometry(parameters),
_decimation(Parameters::defaultOdomICPDecimation()),
_voxelSize(Parameters::defaultOdomICPVoxelSize()),
_samples(Parameters::defaultOdomICPSamples()),
_maxCorrespondenceDistance(Parameters::defaultOdomICPCorrespondencesDistance()),
_maxIterations(Parameters::defaultOdomICPIterations()),
_maxFitness(Parameters::defaultOdomICPMaxFitness()),
_previousCloud(new pcl::PointCloud<pcl::PointNormal>)
{
Parameters::parse(parameters, Parameters::kOdomICPDecimation(), _decimation);
Parameters::parse(parameters, Parameters::kOdomICPVoxelSize(), _voxelSize);
Parameters::parse(parameters, Parameters::kOdomICPSamples(), _samples);
Parameters::parse(parameters, Parameters::kOdomICPCorrespondencesDistance(), _maxCorrespondenceDistance);
Parameters::parse(parameters, Parameters::kOdomICPIterations(), _maxIterations);
Parameters::parse(parameters, Parameters::kOdomICPMaxFitness(), _maxFitness);
}
void OdometryICP::reset()
corelib/src/Rtabmap.cpp
+7 -6
View File
@@ -26,6 +26,7 @@
#include "rtabmap/core/EpipolarGeometry.h"
#include "rtabmap/core/Memory.h"
#include "rtabmap/core/VWDictionary.h"
#include "BayesFilter.h"
#include <rtabmap/utilite/ULogger.h>
@@ -1406,7 +1407,7 @@ bool Rtabmap::process(const Image & image)
{
lcHypothesisReactivated = sLoop->isSaved()?1.0f:0.0f;
}
dictionarySize = _memory->getVWDictionarySize();
dictionarySize = _memory->getVWDictionary()->getVisualWords().size();
refWordsCount = (int)signature->getWords().size();
refUniqueWordsCount = (int)uUniqueKeys(signature->getWords()).size();
@@ -1711,6 +1712,11 @@ bool Rtabmap::process(const Image & image)
return true;
}
bool Rtabmap::process(const cv::Mat & image, int id)
{
return this->process(Image(image, id));
}
// SETTERS
void Rtabmap::setTimeThreshold(float maxTimeAllowed)
{
@@ -1804,11 +1810,6 @@ void Rtabmap::rejectLoopClosure(int oldId, int newId)
}
}
bool Rtabmap::process(const cv::Mat & image, int id)
{
return this->process(Image(image, id));
}
void Rtabmap::dumpData() const
{
UDEBUG("");
corelib/src/VWDictionary.cpp
+520 -692
View File
File diff suppressed because it is too large Load Diff
corelib/src/VisualWord.cpp
+2 -16
View File
@@ -24,22 +24,12 @@
namespace rtabmap
{
VisualWord::VisualWord(int id, const float * descriptor, int dim, int signatureId) :
VisualWord::VisualWord(int id, const cv::Mat & descriptor, int signatureId) :
_id(id),
_descriptor(descriptor),
_saved(false),
_totalReferences(0)
{
_descriptor = new float[dim];
if(_descriptor && descriptor)
{
memcpy(_descriptor, descriptor, dim*sizeof(float));
}
else
{
ULOGGER_ERROR("not enough memory to create the descriptor...");
}
_dim = dim;
if(signatureId)
{
addRef(signatureId);
@@ -48,10 +38,6 @@ VisualWord::VisualWord(int id, const float * descriptor, int dim, int signatureI
VisualWord::~VisualWord()
{
if(_descriptor)
{
delete [] _descriptor;
}
}
void VisualWord::addRef(int signatureId)
corelib/src/VisualWord.h
+3 -5
View File
@@ -31,7 +31,7 @@ class SignatureSurf;
class RTABMAP_EXP VisualWord
{
public:
VisualWord(int id, const float * descriptor, int dim, int signatureId = 0);
VisualWord(int id, const cv::Mat & descriptor, int signatureId = 0);
~VisualWord();
void addRef(int signatureId);
@@ -39,8 +39,7 @@ public:
int getTotalReferences() const {return _totalReferences;}
int id() const {return _id;}
const float * getDescriptor() const {return _descriptor;}
int getDim() const {return _dim;}
const cv::Mat & getDescriptor() const {return _descriptor;}
const std::map<int, int> & getReferences() const {return _references;} // (signature id , occurrence in the signature)
bool isSaved() const {return _saved;}
@@ -48,8 +47,7 @@ public:
private:
int _id;
float * _descriptor;
int _dim;
cv::Mat _descriptor;
bool _saved; // If it's saved to db
int _totalReferences;
corelib/src/util3d.cpp
+20 -1
View File
@@ -377,7 +377,8 @@ void findCorrespondences(
const std::multimap<int, pcl::PointXYZ> & words2,
pcl::PointCloud<pcl::PointXYZ> & inliers1,
pcl::PointCloud<pcl::PointXYZ> & inliers2,
float maxDepth)
float maxDepth,
std::set<int> * uniqueCorrespondences)
{
std::list<int> ids = uUniqueKeys(words1);
// Find pairs
@@ -398,6 +399,10 @@ void findCorrespondences(
(maxDepth <= 0 || (inliers1[oi].x <= maxDepth && inliers2[oi].x<=maxDepth)))
{
++oi;
if(uniqueCorrespondences)
{
uniqueCorrespondences->insert(*iter);
}
}
}
}
@@ -604,6 +609,20 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP transformPointCloud(
return output;
}
pcl::PointXYZ RTABMAP_EXP transformPoint(
const pcl::PointXYZ & pt,
const Transform & transform)
{
return pcl::transformPoint(pt, transformToEigen3f(transform));
}
pcl::PointXYZRGB RTABMAP_EXP transformPoint(
const pcl::PointXYZRGB & pt,
const Transform & transform)
{
return pcl::transformPoint(pt, transformToEigen3f(transform));
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
const cv::Mat & imageDepth,
float depthConstant,
examples/RGBDMapping/main.cpp
+1 -1
View File
@@ -53,7 +53,7 @@ int main(int argc, char * argv[])
}
// Create an odometry thread to process camera events, it will send OdometryEvent.
OdometryThread odomThread(new OdometryBOW(0)); // 0=SURF 1=SIFT
OdometryThread odomThread(new OdometryBOW()); // 0=SURF 1=SIFT
// Create RTAB-Map to process OdometryEvent
guilib/include/rtabmap/gui/OdometryViewer.h
+1
View File
@@ -37,6 +37,7 @@ private:
UMutex dataMutex_;
std::list<rtabmap::Image> data_;
int dataQuality_;
Transform lastOdomPose_;
UTimer timer_;
int maxClouds_;
float voxelSize_;
guilib/include/rtabmap/gui/PreferencesDialog.h
+1 -8
View File
@@ -82,12 +82,6 @@ public:
kSrcOpenNI2
};
enum OdomType {
kOdomBIN,
kOdomBOW,
kOdomICP
};
public:
PreferencesDialog(QWidget * parent = 0);
virtual ~PreferencesDialog();
@@ -148,7 +142,6 @@ public:
bool isSourceImageUsed() const;
bool isSourceDatabaseUsed() const;
bool isSourceOpenniUsed() const;
OdomType getOdometryType() const;
int getSourceImageType() const;
QString getSourceImageTypeStr() const;
int getSourceWidth() const;
@@ -259,7 +252,7 @@ private:
void addParameters(const QGroupBox * box);
QList<QGroupBox*> getGroupBoxes();
void readSettingsBegin();
void testOdometry(OdomType type);
void testOdometry(int type);
protected:
rtabmap::ParametersMap _parameters;
guilib/src/MainWindow.cpp
+2 -32
View File
@@ -1963,22 +1963,7 @@ void MainWindow::startDetection()
UERROR("OdomThread must be already deleted here?!");
delete _odomThread;
}
Odometry * odom;
if(_preferencesDialog->getOdometryType() == PreferencesDialog::kOdomBIN)
{
UINFO("Using odometry FAST/BRIEF");
odom = new OdometryBinary(parameters);
}
else if(_preferencesDialog->getOdometryType() == PreferencesDialog::kOdomBOW)
{
UINFO("Using odometry SIFT/SURF");
odom = new OdometryBOW(parameters);
}
else
{
UINFO("Using odometry ICP");
odom = new OdometryICP(parameters);
}
Odometry * odom = new OdometryBOW(parameters);
_odomThread = new OdometryThread(odom);
UEventsManager::addHandler(_odomThread);
@@ -2057,22 +2042,7 @@ void MainWindow::startDetection()
UERROR("OdomThread must be already deleted here?!");
delete _odomThread;
}
Odometry * odom;
if(_preferencesDialog->getOdometryType() == PreferencesDialog::kOdomBIN)
{
UINFO("Using odometry FAST/BRIEF");
odom = new OdometryBinary(parameters);
}
else if(_preferencesDialog->getOdometryType() == PreferencesDialog::kOdomBOW)
{
UINFO("Using odometry SIFT/SURF");
odom = new OdometryBOW(parameters);
}
else
{
UINFO("Using odometry ICP");
odom = new OdometryICP(parameters);
}
Odometry * odom = new OdometryBOW(parameters);
_odomThread = new OdometryThread(odom);
UEventsManager::addHandler(_odomThread);
guilib/src/OdometryViewer.cpp
+46 -40
View File
@@ -23,6 +23,7 @@ namespace rtabmap {
OdometryViewer::OdometryViewer(int maxClouds, int decimation, float voxelSize, int qualityWarningThr, QWidget * parent) :
CloudViewer(parent),
lastOdomPose_(Transform::getIdentity()),
maxClouds_(maxClouds),
voxelSize_(voxelSize),
decimation_(decimation),
@@ -60,9 +61,9 @@ void OdometryViewer::processData()
}
dataMutex_.unlock();
if(!data.empty() && this->isVisible())
if(!data.image().empty() && !data.depth().empty() && data.depthConstant()>0.0f && this->isVisible())
{
UINFO("New pose = %s", data.pose().prettyPrint().c_str());
UDEBUG("New pose = %s, quality=%d", data.pose().prettyPrint().c_str(), quality);
// visualization: buffering the clouds
// Create the new cloud
@@ -83,30 +84,44 @@ void OdometryViewer::processData()
cloud = util3d::transformPointCloud(cloud, data.localTransform());
data.id()?id_=data.id():++id_;
clouds_.insert(std::make_pair(id_, cloud));
while(maxClouds_>0 && (int)clouds_.size() > maxClouds_)
if(!data.pose().isNull())
{
this->removeCloud(uFormat("cloud%d", clouds_.begin()->first));
clouds_.erase(clouds_.begin());
}
lastOdomPose_ = data.pose();
if(this->getAddedClouds().contains("cloudtmp"))
{
this->removeCloud("cloudtmp");
}
if(clouds_.size())
{
this->addCloud(uFormat("cloud%d", clouds_.rbegin()->first), clouds_.rbegin()->second, data.pose());
}
data.id()?id_=data.id():++id_;
this->updateCameraPosition(data.pose());
clouds_.insert(std::make_pair(id_, cloud));
if(qualityWarningThr_ && quality && quality < qualityWarningThr_)
{
this->setBackgroundColor(Qt::darkYellow);
while(maxClouds_>0 && (int)clouds_.size() > maxClouds_)
{
this->removeCloud(uFormat("cloud%d", clouds_.begin()->first));
clouds_.erase(clouds_.begin());
}
if(clouds_.size())
{
this->addCloud(uFormat("cloud%d", clouds_.rbegin()->first), clouds_.rbegin()->second, data.pose());
}
this->updateCameraPosition(data.pose());
if(qualityWarningThr_ && quality && quality < qualityWarningThr_)
{
this->setBackgroundColor(Qt::darkYellow);
}
else
{
this->setBackgroundColor(Qt::black);
}
}
else
{
this->setBackgroundColor(Qt::black);
this->addOrUpdateCloud("cloudtmp", cloud, lastOdomPose_);
this->setBackgroundColor(Qt::darkRed);
}
this->render();
@@ -121,31 +136,22 @@ void OdometryViewer::handleEvent(UEvent * event)
{
rtabmap::OdometryEvent * odomEvent = (rtabmap::OdometryEvent*)event;
if(odomEvent->isValid())
bool empty = false;
dataMutex_.lock();
if(data_.empty())
{
bool empty = false;
dataMutex_.lock();
if(data_.empty())
{
data_.push_back(odomEvent->data());
empty= true;
}
else
{
data_.back() = odomEvent->data();
}
dataQuality_ = odomEvent->quality();
dataMutex_.unlock();
if(empty)
{
QMetaObject::invokeMethod(this, "processData");
}
data_.push_back(odomEvent->data());
empty= true;
}
else
{
//UWARN("odom=%fs, Cannot compute odometry!!!", timer_.restart());
QMetaObject::invokeMethod(this, "setBackgroundColor", Q_ARG(QColor, Qt::darkRed));
QMetaObject::invokeMethod(this, "render");
data_.back() = odomEvent->data();
}
dataQuality_ = odomEvent->quality();
dataMutex_.unlock();
if(empty)
{
QMetaObject::invokeMethod(this, "processData");
}
}
}
guilib/src/PreferencesDialog.cpp
+153 -120
View File
@@ -42,6 +42,7 @@
#include "rtabmap/core/CameraThread.h"
#include "rtabmap/core/Camera.h"
#include "rtabmap/core/Memory.h"
#include "rtabmap/core/VWDictionary.h"
#include "rtabmap/gui/LoopClosureViewer.h"
@@ -73,8 +74,25 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
if(cv::gpu::getCudaEnabledDeviceCount() == 0)
{
_ui->surf_checkBox_gpuVersion->setChecked(false);
_ui->surf_checkBox_gpuVersion->setEnabled(false);
_ui->label_surf_checkBox_gpuVersion->setEnabled(false);
_ui->surf_doubleSpinBox_gpuKeypointsRatio->setEnabled(false);
_ui->label_surf_checkBox_gpuKeypointsRatio->setEnabled(false);
_ui->fastGpu->setChecked(false);
_ui->fastGpu->setEnabled(false);
_ui->label_fastGPU->setEnabled(false);
_ui->fastKeypointRatio->setEnabled(false);
_ui->label_fastGPUKptRatio->setEnabled(false);
_ui->checkBox_ORBGpu->setChecked(false);
_ui->checkBox_ORBGpu->setEnabled(false);
_ui->label_orbGpu->setEnabled(false);
// remove BruteForceGPU option
_ui->comboBox_dictionary_strategy->removeItem(4);
_ui->odom_bin_nn->removeItem(4);
}
_ui->predictionPlot->showLegend(false);
@@ -317,11 +335,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->comboBox_dictionary_strategy->setObjectName(Parameters::kKpNNStrategy().c_str());
_ui->checkBox_dictionary_incremental->setObjectName(Parameters::kKpIncrementalDictionary().c_str());
_ui->comboBox_detector_strategy->setObjectName(Parameters::kKpDetectorStrategy().c_str());
_ui->checkBox_dictionary_minDistUsed->setObjectName(Parameters::kKpMinDistUsed().c_str());
_ui->surf_doubleSpinBox_matchThr->setObjectName(Parameters::kKpMinDist().c_str());
_ui->checkBox_dictionary_nndrUsed->setObjectName(Parameters::kKpNndrUsed().c_str());
_ui->surf_doubleSpinBox_nndrRatio->setObjectName(Parameters::kKpNndrRatio().c_str());
_ui->surf_spinBox_maxLeafs->setObjectName(Parameters::kKpMaxLeafs().c_str());
_ui->surf_doubleSpinBox_maxDepth->setObjectName(Parameters::kKpMaxDepth().c_str());
_ui->surf_spinBox_wordsPerImageTarget->setObjectName(Parameters::kKpWordsPerImage().c_str());
_ui->surf_doubleSpinBox_ratioBadSign->setObjectName(Parameters::kKpBadSignRatio().c_str());
@@ -338,6 +352,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->checkBox_surfExtended->setObjectName(Parameters::kSURFExtended().c_str());
_ui->surf_checkBox_upright->setObjectName(Parameters::kSURFUpright().c_str());
_ui->surf_checkBox_gpuVersion->setObjectName(Parameters::kSURFGpuVersion().c_str());
_ui->surf_doubleSpinBox_gpuKeypointsRatio->setObjectName(Parameters::kSURFGpuKeypointsRatio().c_str());
//SIFT detector
_ui->sift_spinBox_nFeatures->setObjectName(Parameters::kSIFTNFeatures().c_str());
@@ -346,6 +361,32 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->sift_doubleSpinBox_edgeThr->setObjectName(Parameters::kSIFTEdgeThreshold().c_str());
_ui->sift_doubleSpinBox_sigma->setObjectName(Parameters::kSIFTSigma().c_str());
//BRIEF descriptor
_ui->briefBytes->setObjectName(Parameters::kBRIEFBytes().c_str());
//FAST detector
_ui->fastSuppressNonMax->setObjectName(Parameters::kFASTNonmaxSuppression().c_str());
_ui->fastThreshold->setObjectName(Parameters::kFASTThreshold().c_str());
_ui->fastGpu->setObjectName(Parameters::kFASTGpu().c_str());
_ui->fastKeypointRatio->setObjectName(Parameters::kFASTGpuKeypointsRatio().c_str());
//ORB detector
_ui->spinBox_ORBNFeatures->setObjectName(Parameters::kORBNFeatures().c_str());
_ui->doubleSpinBox_ORBScaleFactor->setObjectName(Parameters::kORBScaleFactor().c_str());
_ui->spinBox_ORBNLevels->setObjectName(Parameters::kORBNLevels().c_str());
_ui->spinBox_ORBEdgeThreshold->setObjectName(Parameters::kORBEdgeThreshold().c_str());
_ui->spinBox_ORBFirstLevel->setObjectName(Parameters::kORBFirstLevel().c_str());
_ui->spinBox_ORBWTA_K->setObjectName(Parameters::kORBWTA_K().c_str());
_ui->spinBox_ORBScoreType->setObjectName(Parameters::kORBScoreType().c_str());
_ui->spinBox_ORBPatchSize->setObjectName(Parameters::kORBPatchSize().c_str());
_ui->checkBox_ORBGpu->setObjectName(Parameters::kORBGpu().c_str());
//FREAK descriptor
_ui->checkBox_FREAKOrientationNormalized->setObjectName(Parameters::kFREAKOrientationNormalized().c_str());
_ui->checkBox_FREAKScaleNormalized->setObjectName(Parameters::kFREAKScaleNormalized().c_str());
_ui->doubleSpinBox_FREAKPatternScale->setObjectName(Parameters::kFREAKPatternScale().c_str());
_ui->spinBox_FREAKNOctaves->setObjectName(Parameters::kFREAKNOctaves().c_str());
// verifyHypotheses
_ui->comboBox_vh_strategy->setObjectName(Parameters::kRtabmapVhStrategy().c_str());
_ui->surf_spinBox_matchCountMinAccepted->setObjectName(Parameters::kVhEpMatchCountMin().c_str());
@@ -393,26 +434,15 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_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_localHistory->setObjectName(Parameters::kOdomLocalHistory().c_str());
_ui->odom_maxFeatures->setObjectName(Parameters::kOdomMaxWords().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_ratio->setObjectName(Parameters::kOdomWordsRatio().c_str());
_ui->stackedWidget_odom->setCurrentIndex(_ui->odom_type->currentIndex());
connect(_ui->odom_type, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_odom, SLOT(setCurrentIndex(int)));
_ui->odom_bin_briefBytes->setObjectName(Parameters::kOdomBinBriefBytes().c_str());
_ui->odom_bin_fastSuppressNonMax->setObjectName(Parameters::kOdomBinFastNonmaxSuppression().c_str());
_ui->odom_bin_fastThreshold->setObjectName(Parameters::kOdomBinFastThreshold().c_str());
_ui->odom_bin_bruteForceMatching->setObjectName(Parameters::kOdomBinBruteForceMatching().c_str());
_ui->odom_icpDecimation->setObjectName(Parameters::kOdomICPDecimation().c_str());
_ui->odom_icpVoxelSize->setObjectName(Parameters::kOdomICPVoxelSize().c_str());
_ui->odom_icpSamples->setObjectName(Parameters::kOdomICPSamples().c_str());
_ui->odom_icpMaxCorrespondenceDistance->setObjectName(Parameters::kOdomICPCorrespondencesDistance().c_str());
_ui->odom_icpIterations->setObjectName(Parameters::kOdomICPIterations().c_str());
_ui->odom_icpMaxFitness->setObjectName(Parameters::kOdomICPMaxFitness().c_str());
_ui->odom_bin_nn->setObjectName(Parameters::kOdomNearestNeighbor().c_str());
_ui->odom_bin_nndrRatio->setObjectName(Parameters::kOdomNNDR().c_str());
setupSignals();
// custom signals
@@ -791,7 +821,6 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
// match the advanced (spin and doubleSpin boxes)
_ui->general_doubleSpinBox_timeThr->setValue(Parameters::defaultRtabmapTimeThr());
_ui->general_doubleSpinBox_hardThr->setValue(Parameters::defaultRtabmapLoopThr());
_ui->surf_doubleSpinBox_hessianThr->setValue(Parameters::defaultSURFHessianThreshold());
_ui->doubleSpinBox_similarityThreshold->setValue(Parameters::defaultMemRehearsalSimilarity());
_ui->general_spinBox_imagesBufferSize->setValue(Parameters::defaultRtabmapImageBufferSize());
_ui->general_spinBox_maxStMemSize->setValue(Parameters::defaultMemSTMSize());
@@ -813,6 +842,21 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
{
this->resetSettings((QGroupBox*)children.at(i));
}
else if(qobject_cast<const QStackedWidget*>(children.at(i)))
{
QStackedWidget * stackedWidget = (QStackedWidget*)children.at(i);
for(int j=0; j<stackedWidget->count(); ++j)
{
const QObjectList & children2 = stackedWidget->widget(j)->children();
for(int k=0; k<children2.size(); ++k)
{
if(qobject_cast<QGroupBox *>(children2.at(k)))
{
this->resetSettings((QGroupBox*)children2.at(k));
}
}
}
}
}
if(groupBox->findChild<QLineEdit*>(_ui->lineEdit_kp_roi->objectName()))
@@ -1294,7 +1338,30 @@ void PreferencesDialog::writeCoreSettings(const QString & filePath)
bool PreferencesDialog::validateForm()
{
//TODO...
//verify odom type vs nearest neighbor approach
if(_ui->comboBox_dictionary_strategy->currentIndex() == VWDictionary::kNNFlannLSH)
{
QMessageBox::warning(this, tr("Parameter warning"),
tr("With the selected feature type (SURF or SIFT), parameter \"Visual Word->Nearest Neighbor\" "
"cannot be LSH (used for binary descriptor). KD-tree is set instead."));
_ui->comboBox_dictionary_strategy->setCurrentIndex(VWDictionary::kNNFlannKdTree);
}
if(_ui->odom_bin_nn->currentIndex() == VWDictionary::kNNFlannLSH && _ui->odom_type->currentIndex() <= 1)
{
QMessageBox::warning(this, tr("Parameter warning"),
tr("With the selected feature type (SURF or SIFT), parameter \"Odometry->Nearest Neighbor\" "
"cannot be LSH (used for binary descriptor). KD-tree is set instead."));
_ui->odom_bin_nn->setCurrentIndex(VWDictionary::kNNFlannKdTree);
}
else if(_ui->odom_bin_nn->currentIndex() == VWDictionary::kNNFlannKdTree && _ui->odom_type->currentIndex() >1)
{
QMessageBox::warning(this, tr("Parameter warning"),
tr("With the selected feature type (ORB, FAST, FREAK or BRIEF), parameter \"Odometry->Nearest Neighbor\" "
"cannot be KD-Tree (used for float descriptor). LSH is set instead."));
_ui->odom_bin_nn->setCurrentIndex(VWDictionary::kNNFlannLSH);
}
return true;
}
@@ -1642,60 +1709,67 @@ void PreferencesDialog::openDatabaseViewer()
void PreferencesDialog::setParameter(const std::string & key, const std::string & value)
{
UDEBUG("%s=%s", key.c_str(), value.c_str());
QObject * obj = _ui->stackedWidget->findChild<QObject*>(key.c_str());
QWidget * obj = _ui->stackedWidget->findChild<QWidget*>(key.c_str());
if(obj)
{
QSpinBox * spin = qobject_cast<QSpinBox *>(obj);
QDoubleSpinBox * doubleSpin = qobject_cast<QDoubleSpinBox *>(obj);
QComboBox * combo = qobject_cast<QComboBox *>(obj);
QCheckBox * check = qobject_cast<QCheckBox *>(obj);
QRadioButton * radio = qobject_cast<QRadioButton *>(obj);
QLineEdit * lineEdit = qobject_cast<QLineEdit *>(obj);
QGroupBox * groupBox = qobject_cast<QGroupBox *>(obj);
bool ok;
if(spin)
if(obj->isEnabled())
{
spin->setValue(QString(value.c_str()).toInt(&ok));
if(!ok)
QSpinBox * spin = qobject_cast<QSpinBox *>(obj);
QDoubleSpinBox * doubleSpin = qobject_cast<QDoubleSpinBox *>(obj);
QComboBox * combo = qobject_cast<QComboBox *>(obj);
QCheckBox * check = qobject_cast<QCheckBox *>(obj);
QRadioButton * radio = qobject_cast<QRadioButton *>(obj);
QLineEdit * lineEdit = qobject_cast<QLineEdit *>(obj);
QGroupBox * groupBox = qobject_cast<QGroupBox *>(obj);
bool ok;
if(spin)
{
UERROR("Conversion failed from \"%s\" for parameter %s", value.c_str(), key.c_str());
spin->setValue(QString(value.c_str()).toInt(&ok));
if(!ok)
{
UERROR("Conversion failed from \"%s\" for parameter %s", value.c_str(), key.c_str());
}
}
}
else if(doubleSpin)
{
doubleSpin->setValue(QString(value.c_str()).toDouble(&ok));
if(!ok)
else if(doubleSpin)
{
UERROR("Conversion failed from \"%s\" for parameter %s", value.c_str(), key.c_str());
doubleSpin->setValue(QString(value.c_str()).toDouble(&ok));
if(!ok)
{
UERROR("Conversion failed from \"%s\" for parameter %s", value.c_str(), key.c_str());
}
}
}
else if(combo)
{
combo->setCurrentIndex(QString(value.c_str()).toInt(&ok));
if(!ok)
else if(combo)
{
UERROR("Conversion failed from \"%s\" for parameter %s", value.c_str(), key.c_str());
combo->setCurrentIndex(QString(value.c_str()).toInt(&ok));
if(!ok)
{
UERROR("Conversion failed from \"%s\" for parameter %s", value.c_str(), key.c_str());
}
}
else if(check)
{
check->setChecked(uStr2Bool(value.c_str()));
}
else if(radio)
{
radio->setChecked(uStr2Bool(value.c_str()));
}
else if(lineEdit)
{
lineEdit->setText(value.c_str());
}
else if(groupBox)
{
groupBox->setChecked(uStr2Bool(value.c_str()));
}
else
{
ULOGGER_WARN("QObject called %s can't be cast to a supported widget", key.c_str());
}
}
else if(check)
{
check->setChecked(uStr2Bool(value.c_str()));
}
else if(radio)
{
radio->setChecked(uStr2Bool(value.c_str()));
}
else if(lineEdit)
{
lineEdit->setText(value.c_str());
}
else if(groupBox)
{
groupBox->setChecked(uStr2Bool(value.c_str()));
}
else
{
ULOGGER_WARN("QObject called %s can't be cast to a supported widget", key.c_str());
UDEBUG("Ignoring parameter %s because it is disabled.", key.c_str());
}
}
else
@@ -1781,35 +1855,29 @@ void PreferencesDialog::addParameter(const QObject * object, int value)
this->addParameters(_ui->groupBox_vh_epipolar2);
}
}
else if(comboBox == _ui->comboBox_detector_strategy)
else if(comboBox == _ui->comboBox_detector_strategy || comboBox == _ui->odom_type)
{
if(value == 0) // 0 surf
if(value == 0) // surf
{
this->addParameters(_ui->groupBox_detector_surf);
_ui->stackedWidget_visualWord->setCurrentIndex(0);
this->addParameters(_ui->groupBox_detector_surf2);
}
else if(value == 1) // 1 sift
else if(value == 1) // sift
{
this->addParameters(_ui->groupBox_detector_sift);
_ui->stackedWidget_visualWord->setCurrentIndex(1);
this->addParameters(_ui->groupBox_detector_sift2);
}
}
else if(comboBox == _ui->odom_type)
{
if(value == 0) // 0 bow
else if(value == 2) // orb
{
this->addParameters(_ui->groupBox_odom_bow);
_ui->stackedWidget_odom->setCurrentIndex(0);
this->addParameters(_ui->groupBox_detector_orb2);
}
else if(value == 1) // 1 fast
else if(value == 3) // fast+freak
{
this->addParameters(_ui->groupBox_odom_fast);
_ui->stackedWidget_odom->setCurrentIndex(1);
this->addParameters(_ui->groupBox_detector_fast2);
this->addParameters(_ui->groupBox_detector_freak2);
}
else if(value == 2) // 1 fast
else if(value == 4) // fast+brief
{
this->addParameters(_ui->groupBox_odom_icp);
_ui->stackedWidget_odom->setCurrentIndex(2);
this->addParameters(_ui->groupBox_detector_fast2);
this->addParameters(_ui->groupBox_detector_brief2);
}
}
else if(comboBox == _ui->globalDetection_icpType)
@@ -2446,18 +2514,6 @@ bool PreferencesDialog::isSourceOpenniUsed() const
return _ui->groupBox_sourceOpenni->isChecked();
}
PreferencesDialog::OdomType PreferencesDialog::getOdometryType() const
{
if(_ui->odom_type->currentIndex() == 0)
{
return kOdomBOW;
}
else if(_ui->odom_type->currentIndex() == 1)
{
return kOdomBIN;
}
return kOdomICP;
}
int PreferencesDialog::getSourceImageType() const
{
@@ -2686,21 +2742,10 @@ void PreferencesDialog::setSLAMMode(bool enabled)
void PreferencesDialog::testOdometry()
{
if(_ui->odom_type->currentIndex() == 0)
{
testOdometry(kOdomBOW);
}
else if(_ui->odom_type->currentIndex() == 1)
{
testOdometry(kOdomBIN);
}
else // ICP
{
testOdometry(kOdomICP);
}
testOdometry(_ui->odom_type->currentIndex());
}
void PreferencesDialog::testOdometry(OdomType type)
void PreferencesDialog::testOdometry(int type)
{
UASSERT(_odomThread == 0 && _cameraThread == 0);
@@ -2750,27 +2795,15 @@ void PreferencesDialog::testOdometry(OdomType type)
if(camera)
{
Odometry * odometry;
ParametersMap parameters = this->getAllParameters();
if(type == kOdomBIN)
{
odometry = new OdometryBinary(parameters);
}
else if(type == kOdomICP)
{
odometry = new OdometryICP(parameters);
}
else // kOdomBOW
{
odometry = new OdometryBOW(parameters);
}
Odometry * odometry = new OdometryBOW(parameters);
_odomThread = new OdometryThread(odometry); // take ownership of odometry
QWidget * window = new QWidget(this, Qt::Popup);
window->setAttribute(Qt::WA_DeleteOnClose);
window->setWindowFlags(Qt::Dialog);
window->setWindowTitle(tr("%1 Odometry viewer").arg(type==kOdomBIN?"Binary":"Bag-of-words"));
window->setWindowTitle(tr("Odometry viewer"));
window->setMinimumWidth(800);
window->setMinimumHeight(600);
connect( window, SIGNAL(destroyed(QObject*)), this, SLOT(cleanOdometryTest()) );
guilib/src/ui/preferencesDialog.ui
+1036 -878
View File
File diff suppressed because it is too large Load Diff
tools/EpipolarGeometry/main.cpp
+5 -6
View File
@@ -192,12 +192,11 @@ int main(int argc, char** argv)
ParametersMap param;
param.insert(ParametersPair(Parameters::kSURFExtended(), "true"));
param.insert(ParametersPair(Parameters::kSURFHessianThreshold(), "100"));
SURFDetector keypointDetector(param);
SURFDescriptor descriptorExtractor(param);
std::vector<cv::KeyPoint> kpts1 = keypointDetector.generateKeypoints(image1);
std::vector<cv::KeyPoint> kpts2 = keypointDetector.generateKeypoints(image2);
cv::Mat descriptors1 = descriptorExtractor.generateDescriptors(image1, kpts1);
cv::Mat descriptors2 = descriptorExtractor.generateDescriptors(image2, kpts2);
SURF detector(param);
std::vector<cv::KeyPoint> kpts1 = detector.generateKeypoints(image1);
std::vector<cv::KeyPoint> kpts2 = detector.generateKeypoints(image2);
cv::Mat descriptors1 = detector.generateDescriptors(image1, kpts1);
cv::Mat descriptors2 = detector.generateDescriptors(image2, kpts2);
UINFO("detect/extract features = %d ms", timer.elapsed());
timer.start();
tools/OdometryViewer/main.cpp
+204 -88
View File
@@ -2,11 +2,13 @@
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UEventsManager.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/core/Odometry.h>
#include <rtabmap/gui/OdometryViewer.h>
#include <rtabmap/core/CameraThread.h>
#include <rtabmap/core/CameraRGBD.h>
#include <rtabmap/core/DBReader.h>
#include <rtabmap/core/VWDictionary.h>
#include <QtGui/QApplication>
void showUsage()
@@ -14,8 +16,9 @@ void showUsage()
printf("\nUsage:\n"
"odometryViewer [options]\n"
"Options:\n"
" -bow # Use bag-of-words odometry (default 0): 0=SURF, 1=SIFT\n"
" -bin Use binary odometry (FAST+BRIEF)\n"
" -o # Odometry type (default 0): 0=SURF, 1=SIFT, 2=ORB, 3=FAST/FREAK, 4=FAST/BRIEF\n"
" -nn # Nearest neighbor strategy (default 1): 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"
"\n"
" -hz #.# Camera rate (default 0, 0 means as fast as the camera can)\n"
@@ -32,10 +35,11 @@ void showUsage()
" -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 1)\n"
"\n"
" -bin_brief_bytes # BRIEF bytes (default 32)\n"
" -bin_fast_thr # FAST threshold (default 30)\n"
" -bin_lsh Use nearest neighbor LSH (default brute force hamming)\n"
" -brief_bytes # BRIEF bytes (default 32)\n"
" -fast_thr # FAST threshold (default 30)\n"
"\n"
" -d # ICP decimation (default 4)\n"
" -v # ICP voxel size (default 0.005)\n"
@@ -45,9 +49,9 @@ void showUsage()
" -debug Log debug messages\n"
"\n"
"Examples:\n"
" odometryViewer -bow 0 SURF example\n"
" odometryViewer -bow 1 SIFT example\n"
" odometryViewer -bin -hz 10 FAST/BRIEF example\n"
" odometryViewer -odom 0 -lh 5000 SURF example\n"
" odometryViewer -odom 1 -lh 10000 SIFT example\n"
" odometryViewer -odom 4 -nn 2 -lh 1000 FAST/BRIEF example\n"
" odometryViewer -icp -in 0.05 -i 30 ICP example\n");
exit(1);
}
@@ -60,8 +64,10 @@ int main (int argc, char * argv[])
// parse arguments
float rate = 0.0;
std::string inputDatabase;
int odomType = 0; // 0=bow 1=bin 2=ICP
int bowType = 0;
int odomType = 0;
bool icp = false;
int nnType =1;
float nndr = 0.7f;
float distance = 0.005;
int maxWords = 0;
int minInliers = 20;
@@ -78,19 +84,53 @@ int main (int argc, char * argv[])
int maxClouds = 10;
int briefBytes = 32;
int fastThr = 30;
bool useLSH = false;
float sec = 0.0f;
bool gpu = false;
int localHistory = 0;
for(int i=1; i<argc; ++i)
{
if(strcmp(argv[i], "-bow") == 0)
if(strcmp(argv[i], "-o") == 0)
{
++i;
if(i < argc)
{
bowType = std::atoi(argv[i]);
odomType = 0;
if(bowType < 0 || bowType > 1)
odomType = std::atoi(argv[i]);
if(odomType < 0 || odomType > 4)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-nn") == 0)
{
++i;
if(i < argc)
{
nnType = std::atoi(argv[i]);
if(nnType < 0 || nnType > 4)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-nndr") == 0)
{
++i;
if(i < argc)
{
nndr = std::atof(argv[i]);
if(nndr < 0.0f)
{
showUsage();
}
@@ -391,7 +431,29 @@ int main (int argc, char * argv[])
}
continue;
}
if(strcmp(argv[i], "-bin_brief_bytes") == 0)
if(strcmp(argv[i], "-gpu") == 0)
{
gpu = true;
continue;
}
if(strcmp(argv[i], "-lh") == 0)
{
++i;
if(i < argc)
{
localHistory = std::atoi(argv[i]);
if(fitness <= 0)
{
showUsage();
}
}
else
{
showUsage();
}
continue;
}
if(strcmp(argv[i], "-brief_bytes") == 0)
{
++i;
if(i < argc)
@@ -408,7 +470,7 @@ int main (int argc, char * argv[])
}
continue;
}
if(strcmp(argv[i], "-bin_fast_thr") == 0)
if(strcmp(argv[i], "-fast_thr") == 0)
{
++i;
if(i < argc)
@@ -425,19 +487,9 @@ int main (int argc, char * argv[])
}
continue;
}
if(strcmp(argv[i], "-bin_lsh") == 0)
{
useLSH = true;
continue;
}
if(strcmp(argv[i], "-bin") == 0)
{
odomType = 1;
continue;
}
if(strcmp(argv[i], "-icp") == 0)
{
odomType = 2;
icp = true;
continue;
}
if(strcmp(argv[i], "-debug") == 0)
@@ -450,6 +502,17 @@ int main (int argc, char * argv[])
showUsage();
}
if(odomType > 1 && nnType == rtabmap::VWDictionary::kNNFlannKdTree)
{
UERROR("You set \"-o %d\" (binary descriptor), you must use \"-nn 2\" (any \"-nn\" other than kNNFlannKdTree)", odomType);
showUsage();
}
else if(odomType <= 1 && nnType == rtabmap::VWDictionary::kNNFlannLSH)
{
UERROR("You set \"-o %d\" (float descriptor), you must use \"-nn 1\" (any \"-nn\" other than kNNFlannLSH)", odomType);
showUsage();
}
if(inputDatabase.size())
{
UINFO("Using database input \"%s\"", inputDatabase.c_str());
@@ -458,78 +521,131 @@ int main (int argc, char * argv[])
{
UINFO("Using OpenNI camera");
}
UINFO("Camera rate = %f Hz", rate);
UINFO("Maximum clouds shown = %d", maxClouds);
UINFO("Delay = %f s", sec);
UINFO("Odometry used = %s", odomType==0?bowType==0?"Bag-of-words SURF":"Bag-of-words SIFT":odomType==1?"Binary (FAST+BRIEF)":"ICP");
UINFO("Inlier/ICP maximum correspondences distance = %f", distance);
UINFO("Max features = %d", maxWords);
UINFO("Min inliers = %d", minInliers);
UINFO("RANSAC/ICP iterations = %d", iterations);
UINFO("Words ratio = %f", wordsRatio);
UINFO("Max depth = %f", maxDepth);
UINFO("Linear update = %f", linearUpdate);
UINFO("Angular update = %f", angularUpdate);
std::string odomName;
if(odomType == 0)
{
odomName = "SURF";
}
else if(odomType == 1)
{
odomName = "SIFT";
}
else if(odomType == 2)
{
odomName = "ORB";
}
else if(odomType == 3)
{
odomName = "FAST+FREAK";
}
else if(odomType == 4)
{
odomName = "FAST+BRIEF";
}
if(icp)
{
odomName= "ICP";
}
std::string nnName;
if(nnType == 0)
{
nnName = "kNNFlannLinear";
}
else if(nnType == 1)
{
nnName = "kNNFlannKdTree";
}
else if(nnType == 2)
{
nnName= "kNNFlannLSH";
}
else if(nnType == 3)
{
nnName= "kNNBruteForce";
}
else if(nnType == 4)
{
nnName= "kNNBruteForceGPU";
}
UINFO("Odometry used = %s", odomName.c_str());
UINFO("Camera rate = %f Hz", rate);
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("Cloud decimation = %d", decimation);
UINFO("Cloud voxel size = %f", voxel);
UINFO("Cloud samples = %d", samples);
UINFO("Cloud fitness = %f", fitness);
UINFO("Binary BRIEF bytes = %d", briefBytes);
UINFO("Binary FAST threshold = %f", fastThr);
UINFO("Binary LSH = %f", useLSH?"true":"false");
UINFO("Local history = %d", localHistory);
QApplication app(argc, argv);
rtabmap::Odometry * odom = 0;
if(odomType == 0)
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::kOdomLocalHistory(), uNumber2Str(localHistory)));
if(!icp)
{
odom = new rtabmap::OdometryBOW(
bowType,
distance,
maxWords,
minInliers,
iterations,
wordsRatio,
maxDepth,
linearUpdate,
angularUpdate,
resetCountdown);
}
else if(odomType == 1)
{
odom = new rtabmap::OdometryBinary(
distance,
maxWords,
minInliers,
iterations,
wordsRatio,
maxDepth,
linearUpdate,
angularUpdate,
resetCountdown,
briefBytes,
fastThr,
true,
!useLSH);
UINFO("Nearest neighbor = %s", nnName.c_str());
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");
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomMaxWords(), uNumber2Str(maxWords)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomWordsRatio(), uNumber2Str(wordsRatio)));
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::kOdomNearestNeighbor(), uNumber2Str(nnType)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomNNDR(), uNumber2Str(nndr)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomType(), 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)
{
UINFO("BRIEF bytes = %d", briefBytes);
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kBRIEFBytes(), uNumber2Str(briefBytes)));
}
odom = new rtabmap::OdometryBOW(parameters);
}
else // ICP
{
odom = new rtabmap::OdometryICP(
decimation,
voxel,
samples,
distance,
iterations,
fitness,
maxDepth,
linearUpdate,
angularUpdate,
resetCountdown);
UINFO("ICP maximum correspondences distance = %f", distance);
UINFO("ICP iterations = %d", iterations);
UINFO("Cloud decimation = %d", decimation);
UINFO("Cloud voxel size = %f", voxel);
UINFO("Cloud samples = %d", samples);
UINFO("Cloud fitness = %f", fitness);
odom = new rtabmap::OdometryICP(decimation, voxel, samples, distance, iterations, fitness);
}
rtabmap::OdometryThread odomThread(odom);
rtabmap::OdometryViewer odomViewer(maxClouds, 2, 0.0);
rtabmap::OdometryViewer odomViewer(maxClouds, 2, 0.0, 50);
UEventsManager::addHandler(&odomThread);
UEventsManager::addHandler(&odomViewer);