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

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

61
corelib/src/odometry/OdometryF2M.cpp
View File

@@ -73,7 +73,6 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
map_(new Signature(-1)),
lastFrame_(new Signature(1)),
lastFrameOldestNewId_(0),
initGravity_(false),
bundleSeq_(0),
sba_(0)
{
@@ -181,27 +180,19 @@ OdometryF2M::~OdometryF2M()
void OdometryF2M::reset(const Transform & initialPose)
{
Odometry::reset(initialPose);
if(!initGravity_)
{
UDEBUG("initialPose=%s", initialPose.prettyPrint().c_str());
Odometry::reset(initialPose);
*lastFrame_ = Signature(1);
*map_ = Signature(-1);
scansBuffer_.clear();
bundleWordReferences_.clear();
bundlePoses_.clear();
bundleLinks_.clear();
bundleModels_.clear();
bundlePoseReferences_.clear();
bundleSeq_ = 0;
lastFrameOldestNewId_ = 0;
}
initGravity_ = false;
}
bool OdometryF2M::canProcessIMU() const
{
return sba_ && sba_->gravitySigma() > 0.0f;
UDEBUG("initialPose=%s", initialPose.prettyPrint().c_str());
Odometry::reset(initialPose);
*lastFrame_ = Signature(1);
*map_ = Signature(-1);
scansBuffer_.clear();
bundleWordReferences_.clear();
bundlePoses_.clear();
bundleLinks_.clear();
bundleModels_.clear();
bundlePoseReferences_.clear();
bundleSeq_ = 0;
lastFrameOldestNewId_ = 0;
}
// return not null transform if odometry is correctly computed
@@ -220,33 +211,9 @@ Transform OdometryF2M::computeTransform(
}
Transform imuT;
if(sba_ && sba_->gravitySigma() > 0.0f && !data.imu().empty())
if(sba_ && sba_->gravitySigma() > 0.0f && !imus().empty())
{
if(imus().empty())
{
UERROR("IMU received doesn't have orientation set, it is ignored. If you are using RTAB-Map standalone, enable IMU filtering in Preferences->Source panel. On ROS, use \"imu_filter_madgwick\" or \"imu_complementary_filter\" packages to compute the orientation.");
}
else
{
imuT = Transform::getTransform(imus(), data.stamp());
if(this->getPose().r11() == 1.0f && this->getPose().r22() == 1.0f && this->getPose().r33() == 1.0f)
{
if(!imuT.isNull())
{
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());
initGravity_ = true;
this->reset(newFramePose);
}
}
}
if(data.imageRaw().empty() && data.laserScanRaw().isEmpty())
{
return output;
}
imuT = Transform::getTransform(imus(), data.stamp());
}
RegistrationInfo regInfo;