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

Merge branch 'master' of github.com:introlab/rtabmap into devel

Browse Source
This commit is contained in:
matlabbe
2016-06-27 12:17:55 -04:00
parent a310b5e864 f69ca56179
commit 1398b71b60
3 changed files with 58 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files

50
corelib/src/Features2d.cpp
View File

@@ -492,30 +492,42 @@ std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, co
UASSERT(image.type() == CV_8UC1);
cv::Mat mask;
if(maskIn.type()==CV_16UC1 || maskIn.type() == CV_32FC1)
if(!maskIn.empty())
{
mask = cv::Mat::zeros(maskIn.rows, maskIn.cols, CV_8UC1);
for(int i=0; i<(int)mask.total(); ++i)
if(maskIn.type()==CV_16UC1 || maskIn.type() == CV_32FC1)
{
float value = 0.0f;
if(maskIn.type()==CV_16UC1)
mask = cv::Mat::zeros(maskIn.rows, maskIn.cols, CV_8UC1);
for(int i=0; i<(int)mask.total(); ++i)
{
if(((unsigned short*)maskIn.data)[i] > 0 &&
((unsigned short*)maskIn.data)[i] < std::numeric_limits<unsigned short>::max())
float value = 0.0f;
if(maskIn.type()==CV_16UC1)
{
value = float(((unsigned short*)maskIn.data)[i])*0.001f;
if(((unsigned short*)maskIn.data)[i] > 0 &&
((unsigned short*)maskIn.data)[i] < std::numeric_limits<unsigned short>::max())
{
value = float(((unsigned short*)maskIn.data)[i])*0.001f;
}
}
else
{
value = ((float*)maskIn.data)[i];
}
if(value>_minDepth &&
(_maxDepth == 0.0f || value <= _maxDepth))
{
((unsigned char*)mask.data)[i] = 255; // ORB uses 255 to handle pyramids
}
}
else
{
value = ((float*)maskIn.data)[i];
}
if(value>_minDepth &&
(_maxDepth == 0.0f || value <= _maxDepth))
{
((unsigned char*)mask.data)[i] = 1;
}
}
else if(maskIn.type()==CV_8UC1)
{
// assume a standard mask
mask = maskIn;
}
else
{
UERROR("Wrong mask type (%d)! Should be 8UC1, 16UC1 or 32FC1.", maskIn.type());
}
}
@@ -527,7 +539,7 @@ std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, co
// Get keypoints
cv::Rect roi = Feature2D::computeRoi(image, _roiRatios);
keypoints = this->generateKeypointsImpl(image, roi.width && roi.height?roi:cv::Rect(0,0,image.cols, image.rows), mask);
UDEBUG("Keypoints extraction time = %f s, keypoints extracted = %d", timer.ticks(), keypoints.size());
UDEBUG("Keypoints extraction time = %f s, keypoints extracted = %d (mask empty=%d)", timer.ticks(), keypoints.size(), mask.empty()?1:0);
limitKeypoints(keypoints, maxFeatures_);

79
corelib/src/Memory.cpp
View File

@@ -451,7 +451,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
else if(_feature2D)
{
_feature2D->parseParameters(parameters);
}
}
Registration::Type regStrategy = Registration::kTypeUndef;
if((iter=parameters.find(Parameters::kRegStrategy())) != parameters.end())
@@ -478,29 +478,29 @@ void Memory::parseParameters(const ParametersMap & parameters)
{
_registrationIcp->parseParameters(parameters);
}
// do this after all parameters are parsed
// SLAM mode vs Localization mode
iter = parameters.find(Parameters::kMemIncrementalMemory());
if(iter != parameters.end())
{
bool value = uStr2Bool(iter->second.c_str());
if(value == false && _incrementalMemory)
{
// From SLAM to localization, change map id
this->incrementMapId();
// The easiest way to make sure that the mapping session is saved
// is to save the memory in the database and reload it.
if((_memoryChanged || _linksChanged) && _dbDriver)
{
UWARN("Switching from Mapping to Localization mode, the database will be saved and reloaded.");
// do this after all parameters are parsed
// SLAM mode vs Localization mode
iter = parameters.find(Parameters::kMemIncrementalMemory());
if(iter != parameters.end())
{
bool value = uStr2Bool(iter->second.c_str());
if(value == false && _incrementalMemory)
{
// From SLAM to localization, change map id
this->incrementMapId();
// The easiest way to make sure that the mapping session is saved
// is to save the memory in the database and reload it.
if((_memoryChanged || _linksChanged) && _dbDriver)
{
UWARN("Switching from Mapping to Localization mode, the database will be saved and reloaded.");
this->init(_dbDriver->getUrl());
UWARN("Switching from Mapping to Localization mode, the database is reloaded!");
}
}
_incrementalMemory = value;
}
UWARN("Switching from Mapping to Localization mode, the database is reloaded!");
}
}
_incrementalMemory = value;
}
}
void Memory::preUpdate()
@@ -2131,7 +2131,7 @@ Transform Memory::computeTransform(
{
Signature tmpFrom = fromS;
Signature tmpTo = toS;
if(_reextractLoopClosureFeatures)
{
UDEBUG("");
@@ -3222,8 +3222,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
}
cv::Mat depthMask;
if(!decimatedData.depthRaw().empty() &&
_feature2D->getType() != Feature2D::kFeatureOrb) // ORB's mask pyramids don't seem to work well
if(!decimatedData.depthRaw().empty())
{
if(imageMono.rows % decimatedData.depthRaw().rows == 0 &&
imageMono.cols % decimatedData.depthRaw().cols == 0 &&
@@ -3254,34 +3253,6 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
(!decimatedData.rightRaw().empty() && decimatedData.stereoCameraModel().isValidForProjection()))
{
keypoints3D = _feature2D->generateKeypoints3D(decimatedData, keypoints);
if(_feature2D->getMinDepth() > 0.0f || _feature2D->getMaxDepth() > 0.0f)
{
UDEBUG("");
//remove all keypoints/descriptors with no valid 3D points
UASSERT((int)keypoints.size() == descriptors.rows &&
keypoints3D.size() == keypoints.size());
std::vector<cv::KeyPoint> validKeypoints(keypoints.size());
std::vector<cv::Point3f> validKeypoints3D(keypoints.size());
cv::Mat validDescriptors(descriptors.size(), descriptors.type());
int oi=0;
for(unsigned int i=0; i<keypoints3D.size(); ++i)
{
if(util3d::isFinite(keypoints3D[i]))
{
validKeypoints[oi] = keypoints[i];
validKeypoints3D[oi] = keypoints3D[i];
descriptors.row(i).copyTo(validDescriptors.row(oi));
++oi;
}
}
UDEBUG("Removed %d invalid 3D points", (int)keypoints3D.size()-oi);
validKeypoints.resize(oi);
validKeypoints3D.resize(oi);
keypoints = validKeypoints;
keypoints3D = validKeypoints3D;
descriptors = validDescriptors.rowRange(0, oi).clone();
}
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);

6
corelib/src/RegistrationVis.cpp
View File

@@ -260,8 +260,7 @@ Transform RegistrationVis::computeTransformationImpl(
}
cv::Mat depthMask;
if(!fromSignature.sensorData().depthRaw().empty() &&
detector->getType() != Feature2D::kFeatureOrb) // ORB's mask pyramids don't seem to work well
if(!fromSignature.sensorData().depthRaw().empty())
{
if(imageFrom.rows % fromSignature.sensorData().depthRaw().rows == 0 &&
imageFrom.cols % fromSignature.sensorData().depthRaw().cols == 0 &&
@@ -433,8 +432,7 @@ Transform RegistrationVis::computeTransformationImpl(
}
cv::Mat depthMask;
if(!toSignature.sensorData().depthRaw().empty() &&
detector->getType() != Feature2D::kFeatureOrb) // ORB's mask pyramids don't seem to work well
if(!toSignature.sensorData().depthRaw().empty())
{
if(imageTo.rows % toSignature.sensorData().depthRaw().rows == 0 &&
imageTo.cols % toSignature.sensorData().depthRaw().cols == 0 &&