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

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

4
corelib/include/rtabmap/core/CameraEvent.h
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() :

162
corelib/include/rtabmap/core/Features2d.h
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;
};
}

9
corelib/include/rtabmap/core/Image.h
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

17
corelib/include/rtabmap/core/Memory.h
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;

82
corelib/include/rtabmap/core/Odometry.h
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:

50
corelib/include/rtabmap/core/Parameters.h
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");

27
corelib/include/rtabmap/core/VWDictionary.h
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

11
corelib/include/rtabmap/core/util3d.h
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,

1
corelib/src/CMakeLists.txt
View File

@@ -22,7 +22,6 @@ SET(SRC_FILES
Parameters.cpp
Signature.cpp
Features2d.cpp
NearestNeighbor.cpp
Transform.cpp
util3d.cpp

12
corelib/src/DBDriverSqlite3.cpp
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

705
corelib/src/Features2d.cpp
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;
}
}

161
corelib/src/Memory.cpp
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)

111
corelib/src/NearestNeighbor.cpp
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;
}
}

76
corelib/src/NearestNeighbor.h
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_ */

531
corelib/src/Odometry.cpp
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()

13
corelib/src/Rtabmap.cpp
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("");

1212
corelib/src/VWDictionary.cpp
View File

File diff suppressed because it is too large Load Diff

18
corelib/src/VisualWord.cpp
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)

8
corelib/src/VisualWord.h
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;

21
corelib/src/util3d.cpp
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,