i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-04 09:07:47 +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/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));
}
}