i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +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

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,