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

Refactored how IMU is used in odometry (if guess is not set, use imu orientation for guess). Changed canProcessIMU() to canProcessAsynIMU() to make it more clear for odometry approaches able to process IMU between image frames (VIO approaches). ZedOC: fixed device closed if imu is not detected.

Browse Source
This commit is contained in:
matlabbe
2021-03-01 19:27:24 -05:00
parent f7bc47572b
commit ea4cc7cb6c
11 changed files with 171 additions and 181 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Odometry.cpp
+34 -17
View File
@@ -284,6 +284,34 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
{
UASSERT_MSG(data.id() >= 0, uFormat("Input data should have ID greater or equal than 0 (id=%d)!", data.id()).c_str());
// cache imu data
if(!data.imu().empty() && !this->canProcessAsyncIMU())
{
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
Transform imuT = Transform(0,0,data.imu().localTransform().theta()) * orientation*data.imu().localTransform().inverse();
if( this->getPose().r11() == 1.0f && this->getPose().r22() == 1.0f && this->getPose().r33() == 1.0f &&
this->framesProcessed() == 0)
{
Eigen::Quaterniond imuQuat = imuT.getQuaterniond();
Transform previous = this->getPose();
Transform newFramePose = Transform(previous.x(), previous.y(), previous.z(), imuQuat.x(), imuQuat.y(), imuQuat.z(), imuQuat.w());
UWARN("Updated initial pose from %s to %s with IMU orientation", previous.prettyPrint().c_str(), newFramePose.prettyPrint().c_str());
this->reset(newFramePose);
}
imus_.insert(std::make_pair(data.stamp(), imuT));
if(imus_.size() > 1000)
{
imus_.erase(imus_.begin());
}
}
}
if(!_imagesAlreadyRectified && !this->canProcessRawImages() && !data.imageRaw().empty())
{
if(data.stereoCameraModel().isValidForRectification())
@@ -386,21 +414,6 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
}
}
// cache imu data
if(!data.imu().empty())
{
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
imus_.insert(std::make_pair(data.stamp(), Transform(0,0,data.imu().localTransform().theta()) * orientation*data.imu().localTransform().inverse()));
if(imus_.size() > 1000)
{
imus_.erase(imus_.begin());
}
}
}
// KITTI datasets start with stamp=0
double dt = previousStamp_>0.0f || (previousStamp_==0.0f && framesProcessed()==1)?data.stamp() - previousStamp_:0.0;
Transform guess = dt>0.0 && guessFromMotion_ && !velocityGuess_.isNull()?Transform::getIdentity():Transform();
@@ -447,7 +460,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
{
guess = guessIn;
}
else if(!data.imu().empty() && !imus_.empty())
else if(!imus_.empty())
{
// replace orientation guess with IMU (if available)
imuCurrentTransform = Transform::getTransform(imus_, data.stamp());
@@ -458,6 +471,10 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
orientation.r11(), orientation.r12(), orientation.r13(), guess.x(),
orientation.r21(), orientation.r22(), orientation.r23(), guess.y(),
orientation.r31(), orientation.r32(), orientation.r33(), guess.z());
if(_force3DoF)
{
guess = guess.to3DoF();
}
}
}
@@ -523,7 +540,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
}
}
}
else
else if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty() || (this->canProcessAsyncIMU() && !data.imu().empty()))
{
t = this->computeTransform(data, guess, info);
}