i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37:46 +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: