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

added new statistics (features extracted current image, local map size) shown in GUI for Odometry

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1719 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-09-25 15:04:35 +00:00
parent c5eedd9a9d
commit ebeb116c0d
11 changed files with 135 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/DBReader.h
+8 -1
View File
@@ -53,7 +53,14 @@ public:
bool init(int startIndex=0);
void setFrameRate(float frameRate);
void getNextImage(cv::Mat & image, cv::Mat & depth, cv::Mat & depth2d, float & fx, float & fy, float & cx, float & cy, Transform & localTransform, Transform & pose);
void getNextImage(cv::Mat & image,
cv::Mat & depth,
cv::Mat & depth2d,
float & fx, float & fy,
float & cx, float & cy,
Transform & localTransform,
Transform & pose,
int & seq);
protected:
virtual void mainLoopBegin();
corelib/include/rtabmap/core/Odometry.h
+4 -4
View File
@@ -54,7 +54,7 @@ class RTABMAP_EXP Odometry
{
public:
virtual ~Odometry() {}
Transform process(SensorData & data, int * quality = 0);
Transform process(SensorData & data, int * quality = 0, int * features = 0, int * localMapSize = 0);
virtual void reset();
bool isLargeEnoughTransform(const Transform & transform);
@@ -72,7 +72,7 @@ public:
int getLocalHistory() const {return _localHistory;}
private:
virtual Transform computeTransform(const SensorData & image, int * quality = 0) = 0;
virtual Transform computeTransform(const SensorData & image, int * quality = 0, int * features = 0, int * localMapSize = 0) = 0;
private:
int _maxFeatures;
@@ -106,7 +106,7 @@ public:
const Memory * getMemory() const {return _memory;}
private:
virtual Transform computeTransform(const SensorData & image, int * quality = 0);
virtual Transform computeTransform(const SensorData & image, int * quality = 0, int * features = 0, int * localMapSize = 0);
private:
Memory * _memory;
@@ -127,7 +127,7 @@ public:
void reset();
private:
virtual Transform computeTransform(const SensorData & image, int * quality = 0);
virtual Transform computeTransform(const SensorData & image, int * quality = 0, int * features = 0, int * localMapSize = 0);
private:
int _decimation;
corelib/include/rtabmap/core/OdometryEvent.h
+9 -2
View File
@@ -37,10 +37,13 @@ class OdometryEvent : public UEvent
{
public:
OdometryEvent(
const SensorData & data, int quality = -1, float time = 0.0f) :
const SensorData & data, int quality = -1, float time = 0.0f, int features = 0, int localMapSize = 0) :
_data(data),
_quality(quality),
_time(time){}
_time(time),
_features(features),
_localMapSize(localMapSize)
{}
virtual ~OdometryEvent() {}
virtual std::string getClassName() const {return "OdometryEvent";}
@@ -48,11 +51,15 @@ public:
const SensorData & data() const {return _data;}
int quality() const {return _quality;}
float time() const {return _time;} // seconds
int features() const {return _features;}
int localMapSize() const {return _localMapSize;}
private:
SensorData _data;
int _quality;
float _time; // seconds
int _features;
int _localMapSize;
};
class OdometryResetEvent : public UEvent
corelib/include/rtabmap/core/Rtabmap.h
+1
View File
@@ -183,6 +183,7 @@ private:
// strategies for a type of signature or configuration.
EpipolarGeometry * _epipolarGeometry;
BayesFilter * _bayesFilter;
ParametersMap _lastParameters;
Memory * _memory;
corelib/src/DBReader.cpp
+7 -4
View File
@@ -134,7 +134,8 @@ void DBReader::mainLoop()
cv::Mat image, depth, depth2d;
float fx,fy,cx,cy;
Transform localTransform, pose;
this->getNextImage(image, depth, depth2d, fx, fy, cx, cy, localTransform, pose);
int seq = 0;
this->getNextImage(image, depth, depth2d, fx, fy, cx, cy, localTransform, pose, seq);
if(!image.empty())
{
if(depth.empty())
@@ -145,7 +146,7 @@ void DBReader::mainLoop()
{
if(!_odometryIgnored)
{
SensorData data(image, depth, depth2d, fx, fy, cx, cy, pose, localTransform);
SensorData data(image, depth, depth2d, fx, fy, cx, cy, pose, localTransform, seq);
this->post(new OdometryEvent(data));
if(pose.isNull())
{
@@ -157,7 +158,7 @@ void DBReader::mainLoop()
else
{
// without odometry
this->post(new CameraEvent(image, depth, depth2d, fx, fy, cx, cy, localTransform));
this->post(new CameraEvent(image, depth, depth2d, fx, fy, cx, cy, localTransform, seq));
}
}
}
@@ -179,7 +180,8 @@ void DBReader::getNextImage(
float & cx,
float & cy,
Transform & localTransform,
Transform & pose)
Transform & pose,
int & seq)
{
if(_dbDriver)
{
@@ -211,6 +213,7 @@ void DBReader::getNextImage(
int mapId;
_dbDriver->getNodeData(*_currentId, imageBytes, depthBytes, depth2dBytes, fx, fy, cx, cy, localTransform);
_dbDriver->getPose(*_currentId, pose, mapId);
seq = *_currentId;
++_currentId;
if(imageBytes.empty())
{
corelib/src/Odometry.cpp
+17 -6
View File
@@ -99,9 +99,9 @@ bool Odometry::isLargeEnoughTransform(const Transform & transform)
fabs(yaw) > _angularUpdate;
}
Transform Odometry::process(SensorData & data, int * quality)
Transform Odometry::process(SensorData & data, int * quality, int * features, int * localMapSize)
{
Transform t = this->computeTransform(data, quality);
Transform t = this->computeTransform(data, quality, features, localMapSize);
if(!t.isNull())
{
_resetCurrentCount = _resetCountdown;
@@ -210,7 +210,7 @@ std::multimap<int,pcl::PointXYZ> OdometryBOW::getLocalMeansMap() const
}
// return not null transform if odometry is correctly computed
Transform OdometryBOW::computeTransform(const SensorData & data, int * quality)
Transform OdometryBOW::computeTransform(const SensorData & data, int * quality, int * features, int * localMapSize)
{
UTimer timer;
Transform output;
@@ -388,6 +388,15 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality)
_memory->emptyTrash();
}
if(features)
{
*features = nFeatures;
}
if(localMapSize)
{
*localMapSize = (int)localMap_.size();
}
UINFO("Odom update time = %fs features=%d inliers=%d/%d local_map=%d[%d] dict=%d nodes=%d",
timer.elapsed(),
nFeatures,
@@ -430,7 +439,7 @@ void OdometryICP::reset()
}
// return not null transform if odometry is correctly computed
Transform OdometryICP::computeTransform(const SensorData & data, int * quality)
Transform OdometryICP::computeTransform(const SensorData & data, int * quality, int * features, int * localMapSize)
{
UTimer timer;
Transform output;
@@ -605,10 +614,12 @@ void OdometryThread::mainLoop()
if(data.isValid())
{
int quality = -1;
int features = -1;
int localMapSize = -1;
UTimer time;
Transform pose = _odometry->process(data, &quality);
Transform pose = _odometry->process(data, &quality, &features, &localMapSize);
data.setPose(pose); // a null pose notify that odometry could not be computed
this->post(new OdometryEvent(data, quality, time.elapsed()));
this->post(new OdometryEvent(data, quality, time.elapsed(), features, localMapSize));
}
}
corelib/src/Rtabmap.cpp
+19 -13
View File
@@ -422,6 +422,11 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
{
_bayesFilter->parseParameters(parameters);
}
for(ParametersMap::const_iterator iter = parameters.begin(); iter!=parameters.end(); ++iter)
{
uInsert(_lastParameters, ParametersPair(iter->first, iter->second));
}
}
std::string Rtabmap::getDatabasePath() const
@@ -1274,19 +1279,20 @@ bool Rtabmap::process(const SensorData & data)
std::string rejectedMsg;
if(_reextractLoopClosureFeatures)
{
ParametersMap customParameters;
customParameters.insert(ParametersPair(Parameters::kLccBowInlierDistance(), uNumber2Str(_memory->getBowInlierDistance())));
customParameters.insert(ParametersPair(Parameters::kLccBowIterations(), uNumber2Str(_memory->getBowIterations())));
customParameters.insert(ParametersPair(Parameters::kLccBowMinInliers(), uNumber2Str(_memory->getBowMinInliers())));
customParameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(_memory->getBowMaxDepth())));
customParameters.insert(ParametersPair(Parameters::kLccBowForce2D(), uNumber2Str(_memory->getBowForce2D())));
customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
customParameters.insert(ParametersPair(Parameters::kMemImageKept(), "false"));
customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
customParameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(_reextractNNType))); // bruteforce
customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(_reextractNNDR)));
customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(_reextractFeatureType))); // FAST/BRIEF
customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(_reextractMaxWords)));
ParametersMap customParameters = _lastParameters;
// override some parameters
uInsert(customParameters, ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
uInsert(customParameters, ParametersPair(Parameters::kMemImageKept(), "false"));
uInsert(customParameters, ParametersPair(Parameters::kMemSTMSize(), "0"));
uInsert(customParameters, ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(_reextractNNType))); // bruteforce
uInsert(customParameters, ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(_reextractNNDR)));
uInsert(customParameters, ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(_reextractFeatureType))); // FAST/BRIEF
uInsert(customParameters, ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(_reextractMaxWords)));
for(ParametersMap::iterator iter = customParameters.begin(); iter!=customParameters.end(); ++iter)
{
UINFO("%s=%s", iter->first.c_str(), iter->second.c_str());
}
Memory memory(customParameters);
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, float time);
void processOdometry(const rtabmap::SensorData & data, int quality, float time, int features, int localMapSize);
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, float);
void odometryReceived(const rtabmap::SensorData &, int, float, int, int);
void thresholdsChanged(int, int);
void stateChanged(MainWindow::State);
void rtabmapEventInitReceived(int status, const QString & info);
guilib/src/DatabaseViewer.cpp
+4 -1
View File
@@ -907,7 +907,10 @@ void DatabaseViewer::detectMoreLoopClosures()
std::multimap<int, rtabmap::Link> links = updateLinksWithModifications(links_);
optimizedPoses = optimizeGraph(ids, poses_, links);
std::multimap<int, int> clusters = util3d::radiusPosesClustering(optimizedPoses, 0.1, 0.1);
std::multimap<int, int> clusters = util3d::radiusPosesClustering(
optimizedPoses,
ui_->doubleSpinBox_detectMore_radius->value(),
ui_->doubleSpinBox_detectMore_angle->value());
int added = 0;
for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!= clusters.end(); ++iter)
{
guilib/src/MainWindow.cpp
+11 -3
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, float)), this, SLOT(processOdometry(rtabmap::SensorData, int, float)));
connect(this, SIGNAL(odometryReceived(rtabmap::SensorData, int, float, int, int)), this, SLOT(processOdometry(rtabmap::SensorData, int, float, int, int)));
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(), odomEvent->time());
emit odometryReceived(odomEvent->data(), odomEvent->quality(), odomEvent->time(), odomEvent->features(), odomEvent->localMapSize());
}
}
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, float time)
void MainWindow::processOdometry(const rtabmap::SensorData & data, int quality, float time, int features, int localMapSize)
{
Transform pose = data.pose();
if(pose.isNull())
@@ -584,6 +584,14 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, int quality,
{
_ui->statsToolBox->updateStat("Odometry/Time/ms", (float)data.id(), (float)time*1000.0f);
}
if(features >=0)
{
_ui->statsToolBox->updateStat("Odometry/Features/", (float)data.id(), (float)features);
}
if(localMapSize >=0)
{
_ui->statsToolBox->updateStat("Odometry/LocalMapSize/", (float)data.id(), (float)localMapSize);
}
if(!pose.isNull())
{
_lastOdomPose = pose;
guilib/src/ui/DatabaseViewer.ui
+53 -1
View File
@@ -7,7 +7,7 @@
<x>0</x>
<y>0</y>
<width>1076</width>
<height>582</height>
<height>665</height>
</rect>
</property>
<property name="windowTitle">
@@ -912,6 +912,58 @@
</property>
</widget>
</item>
<item row="8" column="0">
<widget class="QLabel" name="label_29">
<property name="text">
<string>Detect more LCs: radius</string>
</property>
</widget>
</item>
<item row="9" column="0">
<widget class="QLabel" name="label_30">
<property name="text">
<string>Detect more LCs: angle</string>
</property>
</widget>
</item>
<item row="8" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox_detectMore_radius">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="maximum">
<double>10.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>0.300000000000000</double>
</property>
</widget>
</item>
<item row="9" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox_detectMore_angle">
<property name="suffix">
<string> rad</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="maximum">
<double>3.140000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>0.300000000000000</double>
</property>
</widget>
</item>
</layout>
</item>
<item>