i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +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
corelib/include/rtabmap/core/Memory.h
+1
View File
@@ -246,6 +246,7 @@ private:
float _rehearsalMaxAngle;
bool _rehearsalWeightIgnoredWhileMoving;
bool _useDepthAsMask;
bool _useOdometryFeatures;
int _idCount;
int _idMapCount;
corelib/include/rtabmap/core/Odometry.h
+2 -2
View File
@@ -54,7 +54,7 @@ public:
public:
virtual ~Odometry();
Transform process(const SensorData & data, OdometryInfo * info = 0);
Transform process(SensorData & data, OdometryInfo * info = 0);
virtual void reset(const Transform & initialPose = Transform::getIdentity());
//getters
@@ -63,7 +63,7 @@ public:
const Transform & previousTransform() const {return previousTransform_;}
private:
virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0) = 0;
virtual Transform computeTransform(SensorData & data, OdometryInfo * info = 0) = 0;
void initKalmanFilter();
void updateKalmanFilter(float dt, float & x, float & y, float & z, float & roll, float & pitch, float & yaw);
corelib/include/rtabmap/core/OdometryF2F.h
+1 -1
View File
@@ -46,7 +46,7 @@ public:
const Signature & getRefFrame() const {return refFrame_;}
private:
virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0);
virtual Transform computeTransform(SensorData & image, OdometryInfo * info = 0);
private:
//Parameters:
corelib/include/rtabmap/core/OdometryF2M.h
+1 -1
View File
@@ -45,7 +45,7 @@ public:
const std::multimap<int, cv::Point3f> & getLocalMap() const;
private:
virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0);
virtual Transform computeTransform(SensorData & data, OdometryInfo * info = 0);
private:
//Parameters
corelib/include/rtabmap/core/OdometryMono.h
+1 -1
View File
@@ -43,7 +43,7 @@ public:
virtual void reset(const Transform & initialPose);
private:
virtual Transform computeTransform(const SensorData & data, OdometryInfo * info = 0);
virtual Transform computeTransform(SensorData & data, OdometryInfo * info = 0);
private:
//Parameters:
int flowWinSize_;
corelib/include/rtabmap/core/Parameters.h
+2 -1
View File
@@ -209,6 +209,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Mem, ImageDecimation, int, 1, "Image decimation (>=1) when creating a signature.");
RTABMAP_PARAM(Mem, LaserScanDownsampleStepSize, int, 1, "If > 1, downsample the laser scans when creating a signature.");
RTABMAP_PARAM(Mem, UseDepthAsMask, bool, false, "Use depth image as mask for features detection.");
RTABMAP_PARAM(Mem, UseOdomFeatures, bool, false, "Use odometry features.");
// KeypointMemory (Keypoint-based)
RTABMAP_PARAM(Kp, NNStrategy, int, 1, "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
@@ -269,7 +270,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(GFTT, K, double, 0.04, "");
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, 1, "The number of pyramid levels. The smallest level will have linear size equal to input_image_linear_size/pow(scaleFactor, nlevels).");
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).");
corelib/src/Memory.cpp
+33 -2
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)
corelib/src/Odometry.cpp
+1 -1
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())
{
corelib/src/OdometryF2F.cpp
+5 -1
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()) &&
corelib/src/OdometryF2M.cpp
+6 -1
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();
corelib/src/OdometryMono.cpp
+1 -1
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;
guilib/include/rtabmap/gui/PreferencesDialog.h
+1
View File
@@ -257,6 +257,7 @@ private slots:
void updatePredictionPlot();
void updateKpROI();
void updateG2oVisibility();
void useOdomFeatures();
void changeWorkingDirectory();
void changeDictionaryPath();
void changeOdomBowFixedLocalMapPath();
guilib/src/KeypointItem.cpp
+4 -2
View File
@@ -77,7 +77,8 @@ void KeypointItem::showDescription()
"Hessian = %4\n"
"X = %5\n"
"Y = %6\n"
"Size = %7").arg(_id).arg(_kpt.angle).arg(_kpt.response).arg(_kpt.pt.x).arg(_kpt.pt.y).arg(_kpt.size));
"Size = %7\n"
"Octave = %8").arg(_id).arg(_kpt.angle).arg(_kpt.response).arg(_kpt.pt.x).arg(_kpt.pt.y).arg(_kpt.size).arg(_kpt.octave));
}
else
{
@@ -87,7 +88,8 @@ void KeypointItem::showDescription()
"X = %5\n"
"Y = %6\n"
"Size = %7\n"
"Depth = %8 m").arg(_id).arg(_kpt.angle).arg(_kpt.response).arg(_kpt.pt.x).arg(_kpt.pt.y).arg(_kpt.size).arg(_depth));
"Octave = %8\n"
"Depth = %9 m").arg(_id).arg(_kpt.angle).arg(_kpt.response).arg(_kpt.pt.x).arg(_kpt.pt.y).arg(_kpt.size).arg(_kpt.octave).arg(_depth));
}
_placeHolder->setRect(text->boundingRect());
}
guilib/src/PreferencesDialog.cpp
+30
View File
@@ -510,6 +510,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->spinBox_imageDecimation->setObjectName(Parameters::kMemImageDecimation().c_str());
_ui->general_spinBox_laserScanDownsample->setObjectName(Parameters::kMemLaserScanDownsampleStepSize().c_str());
_ui->checkBox_useDepthAsMask->setObjectName(Parameters::kMemUseDepthAsMask().c_str());
_ui->checkBox_useOdomFeatures->setObjectName(Parameters::kMemUseOdomFeatures().c_str());
// Database
_ui->checkBox_dbInMemory->setObjectName(Parameters::kDbSqlite3InMemory().c_str());
@@ -776,6 +777,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->doubleSpinBox_kp_roi2, SIGNAL(valueChanged(double)), this, SLOT(updateKpROI()));
connect(_ui->doubleSpinBox_kp_roi3, SIGNAL(valueChanged(double)), this, SLOT(updateKpROI()));
connect(_ui->graphOptimization_type, SIGNAL(currentIndexChanged(int)), this, SLOT(updateG2oVisibility()));
connect(_ui->checkBox_useOdomFeatures, SIGNAL(toggled(bool)), this, SLOT(useOdomFeatures()));
//Create a model from the stacked widgets
// This will add all parameters to the parameters Map
@@ -2192,6 +2194,9 @@ void PreferencesDialog::showEvent ( QShowEvent * event )
_ui->groupBox_source0->setEnabled(false);
_ui->groupBox_odometry1->setEnabled(false);
_ui->checkBox_useOdomFeatures->setChecked(false);
_ui->checkBox_useOdomFeatures->setEnabled(false);
this->setWindowTitle(tr("Preferences [Monitoring mode]"));
}
else
@@ -2207,6 +2212,8 @@ void PreferencesDialog::showEvent ( QShowEvent * event )
_ui->groupBox_source0->setEnabled(true);
_ui->groupBox_odometry1->setEnabled(true);
_ui->checkBox_useOdomFeatures->setEnabled(true);
this->setWindowTitle(tr("Preferences"));
}
this->readSettingsBegin();
@@ -3260,6 +3267,29 @@ void PreferencesDialog::updateG2oVisibility()
_ui->groupBox_g2o->setVisible(_ui->graphOptimization_type->currentIndex() == 1);
}
void PreferencesDialog::useOdomFeatures()
{
if(this->isVisible() && _ui->checkBox_useOdomFeatures->isChecked())
{
int r = QMessageBox::question(this, tr("Using odometry features for vocabulary..."),
tr("Do you want to match feature parameters "
"below with corresponding ones used for odometry?"), QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes);
if(r == QMessageBox::Yes)
{
_ui->comboBox_detector_strategy->setCurrentIndex(_ui->reextract_type->currentIndex());
_ui->surf_doubleSpinBox_maxDepth->setValue(_ui->loopClosure_bowMaxDepth->value());
_ui->surf_doubleSpinBox_minDepth->setValue(_ui->loopClosure_bowMinDepth->value());
_ui->surf_spinBox_wordsPerImageTarget->setValue(_ui->reextract_maxFeatures->value());
_ui->checkBox_useDepthAsMask->setChecked(_ui->loopClosure_useDepthAsMask->isChecked());
_ui->lineEdit_kp_roi->setText(_ui->loopClosure_roi->text());
_ui->subpix_winSize_kp->setValue(_ui->subpix_winSize->value());
_ui->subpix_iterations_kp->setValue(_ui->subpix_iterations->value());
_ui->subpix_eps_kp->setValue(_ui->subpix_eps->value());
}
}
}
void PreferencesDialog::changeWorkingDirectory()
{
QString directory = QFileDialog::getExistingDirectory(this, tr("Working directory"), _ui->lineEdit_workingDirectory->text());
guilib/src/ui/preferencesDialog.ui
+86 -59
View File
@@ -63,7 +63,7 @@
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<y>-591</y>
<width>681</width>
<height>2010</height>
</rect>
@@ -4971,6 +4971,33 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<string>Vocabulary</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_12">
<item>
<layout class="QHBoxLayout" name="horizontalLayout_7" stretch="0,1">
<item>
<widget class="QCheckBox" name="checkBox_useOdomFeatures">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_145">
<property name="text">
<string>Use odometry features. Disable to select different feature type. Note that if no features are received, they will be re-extracted (if visual odometry is used, you may want to match the feature type below with the one used for odometry).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
</layout>
</item>
<item>
<widget class="QGroupBox" name="groupBox_8">
<property name="title">
@@ -4979,6 +5006,64 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<layout class="QVBoxLayout" name="verticalLayout_60">
<item>
<layout class="QGridLayout" name="gridLayout_32" columnstretch="0,1">
<item row="3" column="1">
<widget class="QLabel" name="label_22">
<property name="toolTip">
<string>0 means that the response (hessian) threshold
used for the detector will not be adapted.
Otherwise, the threshold is modified to
generate the number of words requested.</string>
</property>
<property name="text">
<string>Maximum words per image (0=no maximum). Setting to -1 will disable features extraction, so disabling loop closure detection indirectly.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_101">
<property name="text">
<string>ROI ratios [left, right, top, bottom] between 0 and 1.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_81">
<property name="toolTip">
<string>0 means that the response (hessian) threshold
used for the detector will not be adapted.
Otherwise, the threshold is modified to
generate the number of words requested.</string>
</property>
<property name="text">
<string>Bad signature ratio (less than Ratio x AverageWordsPerImage = bad).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QLineEdit" name="lineEdit_kp_roi">
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QComboBox" name="comboBox_detector_strategy">
<item>
@@ -5052,38 +5137,6 @@ generate the number of words requested.</string>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_22">
<property name="toolTip">
<string>0 means that the response (hessian) threshold
used for the detector will not be adapted.
Otherwise, the threshold is modified to
generate the number of words requested.</string>
</property>
<property name="text">
<string>Maximum words per image (0=no maximum). Setting to -1 will disable features extraction, so disabling loop closure detection indirectly.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_101">
<property name="text">
<string>ROI ratios [left, right, top, bottom] between 0 and 1.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="8" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_kp_roi1">
<property name="suffix">
@@ -5174,32 +5227,6 @@ generate the number of words requested.</string>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_81">
<property name="toolTip">
<string>0 means that the response (hessian) threshold
used for the detector will not be adapted.
Otherwise, the threshold is modified to
generate the number of words requested.</string>
</property>
<property name="text">
<string>Bad signature ratio (less than Ratio x AverageWordsPerImage = bad).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QLineEdit" name="lineEdit_kp_roi">
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
<item row="7" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_kp_roi0">
<property name="suffix">