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

Updated how odometry guess is done (adapt to time between timestamps) and Kalman filtering (using velocity)

Browse Source
This commit is contained in:
matlabbe
2016-02-27 18:30:00 -05:00
parent a8d1b91083
commit 6d06b66496
16 changed files with 311 additions and 253 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
corelib/src/OdometryF2M.cpp
View File

@@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util3d.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UMath.h"
#include "rtabmap/utilite/UConversion.h"
#include <opencv2/calib3d/calib3d.hpp>
#include <rtabmap/core/OdometryF2M.h>
@@ -167,6 +168,7 @@ void OdometryF2M::reset(const Transform & initialPose)
// return not null transform if odometry is correctly computed
Transform OdometryF2M::computeTransform(
SensorData & data,
const Transform & guess,
OdometryInfo * info)
{
UTimer timer;
@@ -188,9 +190,12 @@ Transform OdometryF2M::computeTransform(
{
if(map_->getWords3().size() && lastFrame_->sensorData().isValid())
{
Transform guess = this->previousTransform().isIdentity()||this->previousTransform().isNull()?Transform():this->getPose()*this->previousTransform();
Signature tmpMap = *map_;
Transform transform = regVis_->computeTransformationMod(tmpMap, *lastFrame_, guess, &regInfo);
Transform transform = regVis_->computeTransformationMod(
tmpMap,
*lastFrame_,
guess.isNull()?Transform():this->getPose()*guess,
&regInfo);
data.setFeatures(lastFrame_->sensorData().keypoints(), lastFrame_->sensorData().descriptors());