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

Added Odom/VarianceFromInliersCount parameter (default false)

Browse Source
This commit is contained in:
matlabbe
2015-09-28 21:45:41 -04:00
parent 3fa6124ad0
commit bf0790f3b9
7 changed files with 54 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
corelib/src/OdometryThread.cpp
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));
}
}