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

Added Features2d::limitKeypoints() with grid options. Odometry: if imu is provided and no guess is provided, the change of orientation of imu is used for rotation guess (overwrite rotation from Odom/GuessFromMotion). OdometryInfo: added gravity errors when imu is used. Preferences: added a second GravitySigma parameter (overwritting Optimizer/GravitySigma for odometry is not negative) for F2M odometry panel.

Browse Source
This commit is contained in:
matlabbe
2020-05-31 11:22:12 -04:00
parent 6e55525a7b
commit 415a2778f1
15 changed files with 186 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Odometry.cpp
+22
View File
@@ -199,6 +199,7 @@ void Odometry::reset(const Transform & initialPose)
previousStamp_ = 0;
distanceTravelled_ = 0;
framesProcessed_ = 0;
imuLastTransform_.setNull();
if(_force3DoF || particleFilters_.size())
{
float x,y,z, roll,pitch,yaw;
@@ -423,10 +424,29 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
}
}
Transform imuCurrentTransform;
if(!guessIn.isNull())
{
guess = guessIn;
}
else if(!data.imu().empty())
{
// replace orientation guess with IMU (if available)
if(!(data.imu().orientation()[0] == 0.0 && data.imu().orientation()[1] == 0.0 && data.imu().orientation()[2] == 0.0))
{
Transform orientation(0,0,0, data.imu().orientation()[0], data.imu().orientation()[1], data.imu().orientation()[2], data.imu().orientation()[3]);
// orientation includes roll and pitch but not yaw in local transform
imuCurrentTransform = Transform(0,0,data.imu().localTransform().theta()) * orientation*data.imu().localTransform().inverse();
if(!imuLastTransform_.isNull())
{
orientation = imuLastTransform_.inverse() * imuCurrentTransform;
guess = Transform(
orientation.r11(), orientation.r12(), orientation.r13(), guess.x(),
orientation.r21(), orientation.r22(), orientation.r23(), guess.y(),
orientation.r31(), orientation.r32(), orientation.r33(), guess.z());
}
}
}
UTimer time;
Transform t;
@@ -688,6 +708,8 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
}
++framesProcessed_;
imuLastTransform_ = imuCurrentTransform;
return _pose *= t; // update
}
else if(_resetCurrentCount > 0)