i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-10 21:40:19 +08:00
Code Issues Packages Projects Releases Wiki Activity

Enabled binary visual words for dictionary (for loop closure detection).

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1677 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-09-09 21:54:32 +00:00
parent e687302ce0
commit c8593df7eb
10 changed files with 102 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/OdometryEvent.h
+5 -2
View File
@@ -37,19 +37,22 @@ class OdometryEvent : public UEvent
{
public:
OdometryEvent(
const SensorData & data, int quality = -1) :
const SensorData & data, int quality = -1, float time = 0.0f) :
_data(data),
_quality(quality) {}
_quality(quality),
_time(time){}
virtual ~OdometryEvent() {}
virtual std::string getClassName() const {return "OdometryEvent";}
bool isValid() const {return !_data.pose().isNull();}
const SensorData & data() const {return _data;}
int quality() const {return _quality;}
float time() const {return _time;} // seconds
private:
SensorData _data;
int _quality;
float _time; // seconds
};
class OdometryResetEvent : public UEvent
corelib/include/rtabmap/core/Parameters.h
+2 -2
View File
@@ -167,7 +167,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Kp, WordsPerImage, int, 400, "");
RTABMAP_PARAM(Kp, BadSignRatio, float, 0.2, "Bad signature ratio (less than Ratio x AverageWordsPerImage = bad).");
RTABMAP_PARAM(Kp, NndrRatio, float, 0.8, "NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)");
RTABMAP_PARAM(Kp, DetectorStrategy, int, 0, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/BRIEF 6=GFTT/BRIEF.");
RTABMAP_PARAM(Kp, DetectorStrategy, int, 0, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF.");
RTABMAP_PARAM(Kp, TfIdfLikelihoodUsed, bool, false, "Use of the td-idf strategy to compute the likelihood.");
RTABMAP_PARAM(Kp, Parallelized, bool, true, "If the dictionary update and signature creation were parallelized.");
RTABMAP_PARAM_STR(Kp, RoiRatios, "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
@@ -251,7 +251,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(RGBD, LocalLoopDetectionMaxDiffID, int, 0, "Maximum ID difference between the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.")
// Odometry
RTABMAP_PARAM(Odom, Type, int, 0, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF.");
RTABMAP_PARAM(Odom, Type, int, 0, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF.");
RTABMAP_PARAM(Odom, LinearUpdate, float, 0.0, "Min linear displacement to update odometry.");
RTABMAP_PARAM(Odom, AngularUpdate, float, 0.0, "Min angular displacement to update odometry.");
RTABMAP_PARAM(Odom, MaxWords, int, 0, "0 no limits.");
corelib/src/DBDriverSqlite3.cpp
+38 -7
View File
@@ -1271,11 +1271,22 @@ void DBDriverSqlite3::loadQuery(VWDictionary * dictionary) const
descriptor = sqlite3_column_blob(ppStmt, index); // VisualWord descriptor array
dRealSize = sqlite3_column_bytes(ppStmt, index++);
if(dRealSize/int(sizeof(float)) != descriptorSize)
cv::Mat d;
if(dRealSize == descriptorSize)
{
UERROR("Saved buffer size (%d) is not the same as descriptor size (%d)", dRealSize/sizeof(float), descriptorSize);
// CV_8U binary descriptors
d = cv::Mat(1, descriptorSize, CV_8U);
}
cv::Mat d(1, descriptorSize, CV_32F);
else if(dRealSize/int(sizeof(float)) == descriptorSize)
{
// CV_32F
d = cv::Mat(1, descriptorSize, CV_32F);
}
else
{
UFATAL("Saved buffer size (%d bytes) is not the same as descriptor size (%d)", dRealSize, descriptorSize);
}
memcpy(d.data, descriptor, dRealSize);
VisualWord * vw = new VisualWord(id, d);
vw->setSaved(true);
@@ -1344,12 +1355,22 @@ void DBDriverSqlite3::loadWordsQuery(const std::set<int> & wordIds, std::list<Vi
descriptor = sqlite3_column_blob(ppStmt, index); // VisualWord descriptor array
dRealSize = sqlite3_column_bytes(ppStmt, index++);
if(dRealSize/int(sizeof(float)) != descriptorSize)
cv::Mat d;
if(dRealSize == descriptorSize)
{
UERROR("Saved buffer size (%d) is not the same as descriptor size (%d)", dRealSize/sizeof(float), descriptorSize);
// CV_8U binary descriptors
d = cv::Mat(1, descriptorSize, CV_8U);
}
else if(dRealSize/int(sizeof(float)) == descriptorSize)
{
// CV_32F
d = cv::Mat(1, descriptorSize, CV_32F);
}
else
{
UFATAL("Saved buffer size (%d bytes) is not the same as descriptor size (%d)", dRealSize, descriptorSize);
}
cv::Mat d(1, descriptorSize, CV_32F);
memcpy(d.data, descriptor, dRealSize);
VisualWord * vw = new VisualWord(*iter, d);
if(vw)
@@ -1848,7 +1869,17 @@ void DBDriverSqlite3::saveQuery(const std::list<VisualWord *> & words) const
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
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().data, w->getDescriptor().cols*sizeof(float), SQLITE_STATIC);
UASSERT(w->getDescriptor().type() == CV_32F || w->getDescriptor().type() == CV_8U);
if(w->getDescriptor().type() == CV_32F)
{
// CV_32F
rc = sqlite3_bind_blob(ppStmt, 3, w->getDescriptor().data, w->getDescriptor().cols*sizeof(float), SQLITE_STATIC);
}
else
{
// CV_8U
rc = sqlite3_bind_blob(ppStmt, 3, w->getDescriptor().data, w->getDescriptor().cols*sizeof(char), SQLITE_STATIC);
}
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
//execute query
corelib/src/Odometry.cpp
+2 -1
View File
@@ -605,9 +605,10 @@ void OdometryThread::mainLoop()
if(data.isValid())
{
int quality = -1;
UTimer time;
Transform pose = _odometry->process(data, &quality);
data.setPose(pose); // a null pose notify that odometry could not be computed
this->post(new OdometryEvent(data, quality));
this->post(new OdometryEvent(data, quality, time.elapsed()));
}
}
corelib/src/VWDictionary.cpp
+2 -2
View File
@@ -286,11 +286,11 @@ void VWDictionary::update()
_flannIndex->build(_dataTree, cv::flann::LinearIndexParams(), type == CV_32F?cvflann::FLANN_DIST_L2:cvflann::FLANN_DIST_HAMMING);
break;
case kNNFlannKdTree:
UASSERT(type == CV_32F);
UASSERT_MSG(type == CV_32F, "To use KdTree dictionary, float descriptors are required!");
_flannIndex->build(_dataTree, cv::flann::KDTreeIndexParams(), cvflann::FLANN_DIST_L2);
break;
case kNNFlannLSH:
UASSERT(type == CV_8U);
UASSERT_MSG(type == CV_8U, "To use LSH dictionary, binary descriptors are required!");
_flannIndex->build(_dataTree, cv::flann::LshIndexParams(12, 20, 2), cvflann::FLANN_DIST_HAMMING);
break;
default:
guilib/include/rtabmap/gui/MainWindow.h
+2 -2
View File
@@ -137,7 +137,7 @@ private slots:
void selectScreenCaptureFormat(bool checked);
void takeScreenshot();
void updateElapsedTime();
void processOdometry(const rtabmap::SensorData & data, int quality);
void processOdometry(const rtabmap::SensorData & data, int quality, float time);
void applyAllPrefSettings();
void applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags);
void applyPrefSettings(const rtabmap::ParametersMap & parameters);
@@ -169,7 +169,7 @@ private slots:
signals:
void statsReceived(const rtabmap::Statistics &);
void odometryReceived(const rtabmap::SensorData &, int);
void odometryReceived(const rtabmap::SensorData &, int, float);
void thresholdsChanged(int, int);
void stateChanged(MainWindow::State);
void rtabmapEventInitReceived(int status, const QString & info);
guilib/src/DatabaseViewer.cpp
+2
View File
@@ -186,6 +186,8 @@ bool DatabaseViewer::openDatabase(const QString & path)
rtabmap::ParametersMap parameters;
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), "false"));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), "false"));
// use BruteForce dictionary because we don't know which type of descriptors are saved in database
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpNNStrategy(), "3"));
memory_ = new rtabmap::Memory();
guilib/src/MainWindow.cpp
+8 -8
View File
@@ -342,7 +342,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
connect(this, SIGNAL(statsReceived(rtabmap::Statistics)), this, SLOT(processStats(rtabmap::Statistics)));
qRegisterMetaType<rtabmap::SensorData>("rtabmap::SensorData");
connect(this, SIGNAL(odometryReceived(rtabmap::SensorData, int)), this, SLOT(processOdometry(rtabmap::SensorData, int)));
connect(this, SIGNAL(odometryReceived(rtabmap::SensorData, int, float)), this, SLOT(processOdometry(rtabmap::SensorData, int, float)));
connect(this, SIGNAL(noMoreImagesReceived()), this, SLOT(stopDetection()));
@@ -531,7 +531,7 @@ void MainWindow::handleEvent(UEvent* anEvent)
!_processingStatistics)
{
_lastOdometryProcessed = false; // if we receive too many odometry events!
emit odometryReceived(odomEvent->data(), odomEvent->quality());
emit odometryReceived(odomEvent->data(), odomEvent->quality(), odomEvent->time());
}
}
else if(anEvent->getClassName().compare("ULogEvent") == 0)
@@ -554,7 +554,7 @@ void MainWindow::handleEvent(UEvent* anEvent)
}
}
void MainWindow::processOdometry(const rtabmap::SensorData & data, int quality)
void MainWindow::processOdometry(const rtabmap::SensorData & data, int quality, float time)
{
Transform pose = data.pose();
if(pose.isNull())
@@ -578,7 +578,11 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, int quality)
}
if(quality >= 0)
{
_ui->statsToolBox->updateStat("/Odom inliers/", (float)data.id(), (float)quality);
_ui->statsToolBox->updateStat("Odometry/Inliers/", (float)data.id(), (float)quality);
}
if(time > 0)
{
_ui->statsToolBox->updateStat("Odometry/Time/ms", (float)data.id(), (float)time*1000.0f);
}
if(!pose.isNull())
{
@@ -1406,10 +1410,6 @@ void MainWindow::processRtabmapEventInit(int status, const QString & info)
{
if((RtabmapEventInit::Status)status == RtabmapEventInit::kInitializing)
{
if(_state == kDetecting)
{
this->pauseDetection();
}
_initProgressDialog->setAutoClose(true, 1);
_initProgressDialog->resetProgress();
_initProgressDialog->show();
guilib/src/PreferencesDialog.cpp
+15 -5
View File
@@ -1345,15 +1345,25 @@ void PreferencesDialog::writeCoreSettings(const QString & filePath)
bool PreferencesDialog::validateForm()
{
//verify odom type vs nearest neighbor approach
if(_ui->comboBox_dictionary_strategy->currentIndex() == VWDictionary::kNNFlannLSH)
//verify binary featrues and nearest neighbor
// BOW dictionary type
if(_ui->comboBox_dictionary_strategy->currentIndex() == VWDictionary::kNNFlannLSH && _ui->comboBox_detector_strategy->currentIndex() <= 1)
{
QMessageBox::warning(this, tr("Parameter warning"),
tr("With the selected feature type (SURF or SIFT), parameter \"Visual Word->Nearest Neighbor\" "
tr("With the selected feature type (SURF or SIFT), parameter \"Visual word->Nearest Neighbor\" "
"cannot be LSH (used for binary descriptor). KD-tree is set instead."));
_ui->comboBox_dictionary_strategy->setCurrentIndex(VWDictionary::kNNFlannKdTree);
}
else if(_ui->comboBox_dictionary_strategy->currentIndex() == VWDictionary::kNNFlannKdTree && _ui->comboBox_detector_strategy->currentIndex() >1)
{
QMessageBox::warning(this, tr("Parameter warning"),
tr("With the selected feature type (ORB, FAST, FREAK or BRIEF), parameter \"Visual word->Nearest Neighbor\" "
"cannot be KD-Tree (used for float descriptor). BruteForce matching is set instead."));
_ui->comboBox_dictionary_strategy->setCurrentIndex(VWDictionary::kNNBruteForce);
}
// odom type
if(_ui->odom_bin_nn->currentIndex() == VWDictionary::kNNFlannLSH && _ui->odom_type->currentIndex() <= 1)
{
QMessageBox::warning(this, tr("Parameter warning"),
@@ -1365,8 +1375,8 @@ bool PreferencesDialog::validateForm()
{
QMessageBox::warning(this, tr("Parameter warning"),
tr("With the selected feature type (ORB, FAST, FREAK or BRIEF), parameter \"Odometry->Nearest Neighbor\" "
"cannot be KD-Tree (used for float descriptor). LSH is set instead."));
_ui->odom_bin_nn->setCurrentIndex(VWDictionary::kNNFlannLSH);
"cannot be KD-Tree (used for float descriptor). BruteForce matching is set instead."));
_ui->odom_bin_nn->setCurrentIndex(VWDictionary::kNNBruteForce);
}
return true;
guilib/src/ui/preferencesDialog.ui
+26 -1
View File
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>21</number>
<number>9</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29">
@@ -3343,6 +3343,31 @@ generate the number of words requested.</string>
<string>SIFT</string>
</property>
</item>
<item>
<property name="text">
<string>ORB</string>
</property>
</item>
<item>
<property name="text">
<string>FAST+FREAK</string>
</property>
</item>
<item>
<property name="text">
<string>FAST+BRIEF</string>
</property>
</item>
<item>
<property name="text">
<string>GFTT+FREAK</string>
</property>
</item>
<item>
<property name="text">
<string>GFTT+BRIEF</string>
</property>
</item>
</widget>
</item>
</layout>