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

Fixed crash when using binary descriptors and maxWords/maxDepth are set

Browse Source 
Added BRISK detector

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1839 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-10-06 19:41:45 +00:00
parent d30c2baf51
commit 4d4c5af6e5
10 changed files with 303 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

70
corelib/src/Features2d.cpp
View File

@@ -100,7 +100,14 @@ void filterKeypointsByDepth(
{
if(indexes[i] == 1)
{
memcpy(newDescriptors.ptr<float>(di++), descriptors.ptr<float>(i), descriptors.cols*sizeof(float));
if(descriptors.type() == CV_32FC1)
{
memcpy(newDescriptors.ptr<float>(di++), descriptors.ptr<float>(i), descriptors.cols*sizeof(float));
}
else // CV_8UC1
{
memcpy(newDescriptors.ptr<char>(di++), descriptors.ptr<char>(i), descriptors.cols*sizeof(char));
}
}
}
descriptors = newDescriptors;
@@ -146,7 +153,14 @@ void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors
kptsTmp[k] = keypoints[iter->second];
if(descriptors.rows)
{
memcpy(descriptorsTmp.ptr<float>(k), descriptors.ptr<float>(iter->second), descriptors.cols*sizeof(float));
if(descriptors.type() == CV_32FC1)
{
memcpy(descriptorsTmp.ptr<float>(k), descriptors.ptr<float>(iter->second), descriptors.cols*sizeof(float));
}
else
{
memcpy(descriptorsTmp.ptr<char>(k), descriptors.ptr<char>(iter->second), descriptors.cols*sizeof(char));
}
}
}
ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, keypoints.size(), kptsTmp.size()?kptsTmp.back().response:0.0f);
@@ -842,4 +856,56 @@ cv::Mat GFTT_FREAK::generateDescriptorsImpl(const cv::Mat & image, std::vector<c
return descriptors;
}
//////////////////////////
//BRISK
//////////////////////////
BRISK::BRISK(const ParametersMap & parameters) :
thresh_(Parameters::defaultBRISKThresh()),
octaves_(Parameters::defaultBRISKOctaves()),
patternScale_(Parameters::defaultBRISKPatternScale()),
brisk_(0)
{
parseParameters(parameters);
}
BRISK::~BRISK()
{
if(brisk_)
{
delete brisk_;
}
}
void BRISK::parseParameters(const ParametersMap & parameters)
{
Parameters::parse(parameters, Parameters::kBRISKThresh(), thresh_);
Parameters::parse(parameters, Parameters::kBRISKOctaves(), octaves_);
Parameters::parse(parameters, Parameters::kBRISKPatternScale(), patternScale_);
if(brisk_)
{
delete brisk_;
brisk_ = 0;
}
brisk_ = new cv::BRISK(thresh_, octaves_, patternScale_);
}
std::vector<cv::KeyPoint> BRISK::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);
brisk_->detect(imgRoi, keypoints); // Opencv keypoints
return keypoints;
}
cv::Mat BRISK::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{
UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
cv::Mat descriptors;
brisk_->compute(image, keypoints, descriptors);
return descriptors;
}
}

59
corelib/src/Memory.cpp
View File

@@ -455,6 +455,10 @@ void Memory::parseParameters(const ParametersMap & parameters)
_feature2D = new GFTT_BRIEF(parameters);
_featureType = Feature2D::kFeatureGfttBrief;
break;
case Feature2D::kFeatureBrisk:
_feature2D = new BRISK(parameters);
_featureType = Feature2D::kFeatureBrisk;
break;
case Feature2D::kFeatureSurf:
default:
_feature2D = new SURF(parameters);
@@ -3095,7 +3099,7 @@ private:
Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
{
UASSERT(data.image().empty() || data.image().type() == CV_8UC1 || data.image().type() == CV_8UC3);
UASSERT(data.depth().empty() || data.depth().type() == CV_16UC1);
UASSERT(data.depth().empty() || data.depth().type() == CV_16UC1 || data.depth().type() == CV_32FC1);
UASSERT(data.depth2d().empty() || data.depth2d().type() == CV_32FC2);
PreUpdateThread preUpdateThread(_vwd);
@@ -3147,24 +3151,44 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
preUpdateThread.start();
}
// Extract features
cv::Mat imageMono;
// convert to grayscale
if(data.image().channels() > 1)
if(data.keypoints().size() == 0)
{
cv::cvtColor(data.image(), imageMono, cv::COLOR_BGR2GRAY);
// Extract features
cv::Mat imageMono;
// convert to grayscale
if(data.image().channels() > 1)
{
cv::cvtColor(data.image(), imageMono, cv::COLOR_BGR2GRAY);
}
else
{
imageMono = data.image();
}
this->extractKeypointsAndDescriptors(imageMono,
data.depth(),
data.depthFx(), data.depthFy(),
data.depthCx(), data.depthCy(),
keypoints,
descriptors);
UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
{
descriptors = cv::Mat();
}
}
else
{
imageMono = data.image();
}
keypoints = data.keypoints();
descriptors = data.descriptors().clone();
this->extractKeypointsAndDescriptors(imageMono, data.depth(), data.depthFx(), data.depthFy(), data.depthCx(), data.depthCy(), keypoints, descriptors);
UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
{
descriptors = cv::Mat();
filterKeypointsByDepth(keypoints, descriptors,
data.depth(),
data.depthFx(), data.depthFy(),
data.depthCx(), data.depthCy(),
_wordsMaxDepth);
limitKeypoints(keypoints, descriptors, _wordsPerImageTarget);
}
if(_parallelized)
@@ -3215,8 +3239,13 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
{
std::vector<unsigned char> imageBytes;
std::vector<unsigned char> depthBytes;
if(data.depth().type() == CV_32FC1)
{
UWARN("Keeping raw data in database: depth type is 32FC1, use 16UC1 depth format to avoid a conversion.");
}
cv::Mat depthMM = data.depth().type() == CV_32FC1?util3d::cvtDepthFromFloat(data.depth()):data.depth();
util3d::CompressionThread ctImage(data.image(), std::string(".jpg"));
util3d::CompressionThread ctDepth(data.depth(), std::string(".png"));
util3d::CompressionThread ctDepth(depthMM, std::string(".png"));
ctImage.start();
ctDepth.start();
ctImage.join();

27
corelib/src/Odometry.cpp
View File

@@ -154,7 +154,8 @@ OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
group.compare("FAST") == 0 ||
group.compare("ORB") == 0 ||
group.compare("FREAK") == 0 ||
group.compare("GFTT") == 0)
group.compare("GFTT") == 0 ||
group.compare("BRISK") == 0)
{
customParameters.insert(*iter);
}
@@ -256,6 +257,8 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality,
0,
&uniqueCorrespondences);
UDEBUG("localMap=%d, new=%d, unique correspondences=%d", (int)localMeansMap.size(), (int)newSignature->getWords3().size(), (int)uniqueCorrespondences.size());
if((int)inliers1->size() >= this->getMinInliers())
{
correspondences = inliers1->size();
@@ -266,6 +269,22 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality,
this->getInlierDistance(),
this->getIterations(),
&inliers);
/*
//refine ICP test
bool hasConverged;
double fitness;
inliers2 = util3d::transformPointCloud(inliers2, transform);
Transform icpT = util3d::icp(inliers1,
inliers2,
0.02,
100,
hasConverged,
fitness);
transform = transform * icpT;
*/
if(quality)
{
@@ -371,6 +390,7 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality,
localMap_.clear();
output.setIdentity();
int count = 0;
std::list<int> uniques = uUniqueKeys(newSignature->getWords3());
for(std::list<int>::iterator iter = uniques.begin(); iter!=uniques.end(); ++iter)
{
@@ -381,8 +401,13 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality,
{
localMap_.insert(std::make_pair(*iter, std::make_pair(newSignature->id(), pt)));
}
else
{
++count;
}
}
}
UDEBUG("uniques=%d, pt not finite = %d", (int)uniques.size(),count);
}
_memory->emptyTrash();

2
corelib/src/VWDictionary.cpp
View File

@@ -378,7 +378,7 @@ void VWDictionary::removeAllWordRef(int wordId, int signatureId)
std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
int signatureId)
{
UDEBUG("");
UDEBUG("id=%d descriptors=%d", signatureId, descriptors.rows);
UTimer timer;
std::list<int> wordIds;
if(descriptors.rows == 0 || descriptors.cols == 0)

13
corelib/src/util3d.cpp
View File

@@ -1136,11 +1136,6 @@ Transform transformFromXYZCorrespondences(
Transform transform;
if(cloud1->size() && cloud1->size() == cloud2->size())
{
// Not robust to outliers...
//Eigen::Matrix4f transformMatrix;
//pcl::registration::TransformationEstimationSVD<pcl::PointXYZ, pcl::PointXYZ> trans_est;
//trans_est.estimateRigidTransformation (*cloud2, *cloud1, transformMatrix);
// Robust to outliers RANSAC
pcl::CorrespondencesPtr correspondences(new pcl::Correspondences);
for(unsigned int i = 0; i<cloud1->size(); ++i)
@@ -1159,6 +1154,14 @@ Transform transformFromXYZCorrespondences(
UDEBUG("RANSAC inliers=%d outliers=%d", (int)correspondencesInliers.size(), (int)correspondences->size()-(int)correspondencesInliers.size());
transform = util3d::transformFromEigen4f(crsc.getBestTransformation());
/*UDEBUG("RANSAC=%s", transform.prettyPrint().c_str());
pcl::registration::TransformationEstimationSVD<pcl::PointXYZ, pcl::PointXYZ> trans_est;
Eigen::Matrix4f transform_svd;
trans_est.estimateRigidTransformation (*cloud2, *cloud1, correspondencesInliers, transform_svd);
transform = util3d::transformFromEigen4f(transform_svd);
UDEBUG("SVD=%s", transform.prettyPrint().c_str());*/
if(correspondencesInliers.size() == correspondences->size() && transform.isIdentity())
{