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

Added parameter "Mem/UseOdometryFeatures=false" to use directly features already extracted from odometry for the vocabulary (issue #32)

Browse Source
This commit is contained in:
matlabbe
2016-02-22 17:57:40 -05:00
parent ccc4b4a6c2
commit d7ef170d33
15 changed files with 175 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files

35
corelib/src/Memory.cpp
View File

@@ -90,6 +90,7 @@ Memory::Memory(const ParametersMap & parameters) :
_rehearsalMaxAngle(Parameters::defaultRGBDAngularUpdate()),
_rehearsalWeightIgnoredWhileMoving(Parameters::defaultMemRehearsalWeightIgnoredWhileMoving()),
_useDepthAsMask(Parameters::defaultMemUseDepthAsMask()),
_useOdometryFeatures(Parameters::defaultMemUseOdomFeatures()),
_idCount(kIdStart),
_idMapCount(kIdStart),
_lastSignature(0),
@@ -404,6 +405,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), _rehearsalMaxAngle);
Parameters::parse(parameters, Parameters::kMemRehearsalWeightIgnoredWhileMoving(), _rehearsalWeightIgnoredWhileMoving);
Parameters::parse(parameters, Parameters::kMemUseDepthAsMask(), _useDepthAsMask);
Parameters::parse(parameters, Parameters::kMemUseOdomFeatures(), _useOdometryFeatures);
UASSERT_MSG(_maxStMemSize >= 0, uFormat("value=%d", _maxStMemSize).c_str());
UASSERT_MSG(_similarityThreshold >= 0.0f && _similarityThreshold <= 1.0f, uFormat("value=%f", _similarityThreshold).c_str());
@@ -3102,10 +3104,11 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
}
std::vector<cv::Point3f> keypoints3D;
if(data.keypoints().size() == 0)
if(!_useOdometryFeatures || data.keypoints().size() == 0)
{
if(_feature2D->getMaxFeatures() >= 0 && !data.imageRaw().empty() && !isIntermediateNode)
{
UINFO("Extract features");
cv::Mat imageMono;
if(data.imageRaw().channels() == 3)
{
@@ -3166,10 +3169,38 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
}
else if(!isIntermediateNode)
{
UINFO("Use odometry features");
keypoints = data.keypoints();
descriptors = data.descriptors().clone();
keypoints3D = _feature2D->generateKeypoints3D(data, keypoints);
UASSERT(descriptors.empty() || descriptors.rows == (int)keypoints.size());
if(keypoints.size() > _feature2D->getMaxFeatures())
{
_feature2D->limitKeypoints(keypoints, descriptors, _feature2D->getMaxFeatures());
}
if(descriptors.empty())
{
descriptors = _feature2D->generateDescriptors(data.imageRaw(), keypoints);
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
}
UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
{
descriptors = cv::Mat();
}
else if((!data.depthRaw().empty() && data.cameraModels().size() && data.cameraModels()[0].isValidForProjection()) ||
(!data.rightRaw().empty() && data.stereoCameraModel().isValidForProjection()))
{
keypoints3D = _feature2D->generateKeypoints3D(data, keypoints);
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);
}
}
if(_parallelized)

2
corelib/src/Odometry.cpp
View File

@@ -192,7 +192,7 @@ void Odometry::reset(const Transform & initialPose)
}
}
Transform Odometry::process(const SensorData & data, OdometryInfo * info)
Transform Odometry::process(SensorData & data, OdometryInfo * info)
{
if(_pose.isNull())
{

6
corelib/src/OdometryF2F.cpp
View File

@@ -59,7 +59,7 @@ void OdometryF2F::reset(const Transform & initialPose)
// return not null transform if odometry is correctly computed
Transform OdometryF2F::computeTransform(
const SensorData & data,
SensorData & data,
OdometryInfo * info)
{
UTimer timer;
@@ -87,6 +87,8 @@ Transform OdometryF2F::computeTransform(
guessFromMotion_?motionSinceLastKeyFrame_*this->previousTransform():Transform(),
&regInfo);
data.setFeatures(newFrame.sensorData().keypoints(), newFrame.sensorData().descriptors());
if(info && this->isInfoDataFilled())
{
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
@@ -140,6 +142,8 @@ Transform OdometryF2F::computeTransform(
newFrame,
dummy);
features = (int)newFrame.sensorData().keypoints().size();
data.setFeatures(newFrame.sensorData().keypoints(), newFrame.sensorData().descriptors());
}
if((features >= registrationPipeline_->getMinVisualCorrespondences()) &&

7
corelib/src/OdometryF2M.cpp
View File

@@ -168,7 +168,7 @@ const std::multimap<int, cv::Point3f> & OdometryF2M::getLocalMap() const
// return not null transform if odometry is correctly computed
Transform OdometryF2M::computeTransform(
const SensorData & data,
SensorData & data,
OdometryInfo * info)
{
UTimer timer;
@@ -191,6 +191,8 @@ Transform OdometryF2M::computeTransform(
Transform guess = this->previousTransform().isIdentity()||this->previousTransform().isNull()?Transform():this->getPose()*this->previousTransform();
Transform transform = regVis_->computeTransformationMod(*map_, newSignature, guess, &regInfo);
data.setFeatures(newSignature.sensorData().keypoints(), newSignature.sensorData().descriptors());
if(!transform.isNull())
{
// make it incremental
@@ -271,6 +273,8 @@ Transform OdometryF2M::computeTransform(
newSignature,
dummy);
data.setFeatures(newSignature.sensorData().keypoints(), newSignature.sensorData().descriptors());
if(fixedMapPath_.empty() && (int)newSignature.getWords3().size() >= regVis_->getMinInliers())
{
output.setIdentity();
@@ -293,6 +297,7 @@ Transform OdometryF2M::computeTransform(
map_->sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat()); // clear sensorData features
nFeatures = newSignature.getWords().size();
if(this->isInfoDataFilled() && info)
{
info->words = newSignature.getWords();

2
corelib/src/OdometryMono.cpp
View File

@@ -170,7 +170,7 @@ void OdometryMono::reset(const Transform & initialPose)
keyFramePoses_.clear();
}
Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo * info)
Transform OdometryMono::computeTransform(SensorData & data, OdometryInfo * info)
{
Transform output;