i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-06 17:57:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of github.com:introlab/rtabmap

Browse Source
This commit is contained in:
matlabbe
2015-09-29 23:48:21 -04:00
parent fccaa11de1 bf0790f3b9
commit e193aed720
7 changed files with 54 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/OdometryThread.cpp
+13 -2
View File
@@ -34,9 +34,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
OdometryThread::OdometryThread(Odometry * odometry, unsigned int dataBufferMaxSize) :
OdometryThread::OdometryThread(Odometry * odometry, unsigned int dataBufferMaxSize, bool varianceFromInliersCount) :
_odometry(odometry),
_dataBufferMaxSize(dataBufferMaxSize),
_varianceFromInliersCount(varianceFromInliersCount),
_resetOdometry(false)
{
UASSERT(_odometry != 0);
@@ -98,7 +99,17 @@ void OdometryThread::mainLoop()
OdometryInfo info;
Transform pose = _odometry->process(data, &info);
// a null pose notify that odometry could not be computed
double variance = info.variance>0?info.variance:1;
double variance;
if(_varianceFromInliersCount)
{
variance = info.inliers > 0?1.0/double(info.inliers):1.0;
}
else
{
variance = info.variance>0?info.variance:1.0;
}
this->post(new OdometryEvent(data, pose, variance, variance, info));
}
}