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 imu to odom bundle adjustment. Added IMUFilter classes. Changed Aruco parameter prefix to Marker. Zed: publishing IMU data.

Browse Source
This commit is contained in:
matlabbe
2019-05-07 18:57:53 -04:00
parent 4675240d6e
commit e6f471d88e
32 changed files with 1733 additions and 158 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -72,6 +72,7 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
map_(new Signature(-1)),
lastFrame_(new Signature(1)),
lastFrameOldestNewId_(0),
initGravity_(false),
bundleSeq_(0),
sba_(0)
{
@@ -150,6 +151,7 @@ OdometryF2M::~OdometryF2M()
bundleLinks_.clear();
bundleModels_.clear();
bundlePoseReferences_.clear();
imus_.clear();
delete sba_;
delete regPipeline_;
UDEBUG("");
@@ -158,26 +160,33 @@ OdometryF2M::~OdometryF2M()
void OdometryF2M::reset(const Transform & initialPose)
{
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;
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;
imus_.clear();
}
initGravity_ = false;
}
// return not null transform if odometry is correctly computed
Transform OdometryF2M::computeTransform(
SensorData & data,
const Transform & guess,
const Transform & guessIn,
OdometryInfo * info)
{
Transform guess = guessIn;
UTimer timer;
Transform output;
@@ -186,6 +195,39 @@ Transform OdometryF2M::computeTransform(
info->type = 0;
}
if(!data.imu().empty())
{
if(data.imu().orientation()[0] == 0.0 && data.imu().orientation()[1] == 0.0 && data.imu().orientation()[2] == 0.0)
{
UERROR("IMU received doesn't have orientation set, it is ignored.");
}
else
{
Transform orientation(0,0,0, data.imu().orientation()[0], data.imu().orientation()[1], data.imu().orientation()[2], data.imu().orientation()[3]);
//UWARN("%fs %s", data.stamp(), orientation.prettyPrint().c_str());
imus_.insert(std::make_pair(data.stamp(), orientation*data.imu().localTransform().inverse()));
if(imus_.size() > 1000)
{
imus_.erase(imus_.begin());
}
if(this->getPose().r11() == 1.0f && this->getPose().r22() == 1.0f && this->getPose().r33() == 1.0f)
{
Eigen::Quaterniond imuQuat = imus_.rbegin()->second.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;
}
}
RegistrationInfo regInfo;
int nFeatures = 0;
@@ -285,6 +327,21 @@ Transform OdometryF2M::computeTransform(
UDEBUG("Registration time = %fs", regInfo.totalTime);
if(!transform.isNull())
{
Transform imuT;
if(!imus_.empty())
{
double stampDiff = 0.0;
imuT = getClosestIMU(lastFrame_->getStamp(), stampDiff);
if(stampDiff < 0.05)
{
bundleLinks.insert(std::make_pair(lastFrame_->id(), Link(lastFrame_->id(), lastFrame_->id(), Link::kPoseOdom, imuT)));
}
else
{
UWARN("IMUs are set, but we could not find one matching the current frame stamp %f (stampDiff=%f > 0.05)", lastFrame_->getStamp(), stampDiff);
}
}
// local bundle adjustment
if(bundleAdjustment_>0 && sba_ &&
regPipeline_->isImageRequired() &&
@@ -311,6 +368,7 @@ Transform OdometryF2M::computeTransform(
bundlePoses = bundlePoses_;
bundleLinks = bundleLinks_;
bundleModels = bundleModels_;
bundleLinks.insert(bundleIMUOrientations_.begin(), bundleIMUOrientations_.end());
UASSERT_MSG(bundlePoses.find(lastFrame_->id()) == bundlePoses.end(),
uFormat("Frame %d already added! Make sure the input frames have unique IDs!", lastFrame_->id()).c_str());
@@ -320,6 +378,11 @@ Transform OdometryF2M::computeTransform(
bundleLinks.insert(std::make_pair(bundlePoses_.rbegin()->first, Link(bundlePoses_.rbegin()->first, lastFrame_->id(), Link::kNeighbor, bundlePoses_.rbegin()->second.inverse()*transform, var.inv())));
bundlePoses.insert(std::make_pair(lastFrame_->id(), transform));
if(!imuT.isNull())
{
bundleLinks.insert(std::make_pair(lastFrame_->id(), Link(lastFrame_->id(), lastFrame_->id(), Link::kPoseOdom, imuT)));
}
CameraModel model;
if(lastFrame_->sensorData().cameraModels().size() == 1 && lastFrame_->sensorData().cameraModels().at(0).isValidForProjection())
{
@@ -445,7 +508,9 @@ Transform OdometryF2M::computeTransform(
else
{
transform = bundlePoses.rbegin()->second;
bundleLinks.find(bundlePoses_.rbegin()->first)->second.setTransform(bundlePoses_.rbegin()->second.inverse()*transform);
std::multimap<int, Link>::iterator iter = graph::findLink(bundleLinks, bundlePoses_.rbegin()->first, lastFrame_->id(), false);
UASSERT(iter != bundleLinks.end());
iter->second.setTransform(bundlePoses_.rbegin()->second.inverse()*transform);
}
}
UDEBUG("Local Bundle Adjustment After : %s", transform.prettyPrint().c_str());
@@ -534,8 +599,14 @@ Transform OdometryF2M::computeTransform(
if(bundleAdjustment_>0)
{
bundlePoseReferences_.insert(std::make_pair(lastFrame_->id(), 0));
UASSERT(graph::findLink(bundleLinks, bundlePoses_.rbegin()->first, lastFrame_->id(), false) != bundleLinks.end());
bundleLinks_.insert(*bundleLinks.find(bundlePoses_.rbegin()->first));
std::multimap<int, Link>::iterator iter = graph::findLink(bundleLinks, bundlePoses_.rbegin()->first, lastFrame_->id(), false);
UASSERT(iter != bundleLinks.end());
bundleLinks_.insert(*iter);
iter = graph::findLink(bundleLinks, lastFrame_->id(), lastFrame_->id(), false);
if(iter != bundleLinks.end())
{
bundleIMUOrientations_.insert(*iter);
}
uInsert(bundlePoses_, bundlePoses);
UASSERT(bundleModels.find(lastFrame_->id()) != bundleModels.end());
bundleModels_.insert(*bundleModels.find(lastFrame_->id()));
@@ -818,6 +889,7 @@ Transform OdometryF2M::computeTransform(
UASSERT(bundlePoses_.erase(iter->first) == 1);
bundleLinks_.erase(iter->first);
bundleModels_.erase(iter->first);
bundleIMUOrientations_.erase(iter->first);
bundlePoseReferences_.erase(iter++);
}
}
@@ -1012,6 +1084,7 @@ Transform OdometryF2M::computeTransform(
bool frameValid = false;
Transform newFramePose = this->getPose(); // initial pose may be not identity...
if(regPipeline_->isImageRequired())
{
int ptsWithDepth = 0;
@@ -1115,6 +1188,11 @@ Transform OdometryF2M::computeTransform(
}
bundleModels_.insert(std::make_pair(lastFrame_->id(), model));
bundlePoses_.insert(std::make_pair(lastFrame_->id(), newFramePose));
if(!imus_.empty())
{
bundleIMUOrientations_.insert(std::make_pair(lastFrame_->id(), Link(lastFrame_->id(), lastFrame_->id(), Link::kPoseOdom, newFramePose)));
}
}
map_->setWords(words);
@@ -1229,4 +1307,41 @@ Transform OdometryF2M::computeTransform(
return output;
}
Transform OdometryF2M::getClosestIMU(const double & stamp, double & stampDiff) const
{
UASSERT(!imus_.empty());
std::map<double, Transform>::const_iterator imuIterB = imus_.lower_bound(stamp);
std::map<double, Transform>::const_iterator imuIterA = imuIterB;
if(imuIterA != imus_.begin())
{
imuIterA = --imuIterA;
}
if(imuIterB == imus_.end())
{
imuIterB = --imuIterB;
}
Transform imuT;
stampDiff = 0.0;
if(imuIterB->first == lastFrame_->getStamp() || imuIterA == imuIterB)
{
imuT = imuIterB->second;
stampDiff = fabs(imuIterB->first - lastFrame_->getStamp());
}
else if(imuIterA != imuIterB)
{
if(fabs(imuIterA->first - lastFrame_->getStamp()) <
fabs(imuIterB->first - lastFrame_->getStamp()))
{
imuT = imuIterA->second;
stampDiff = fabs(imuIterA->first - lastFrame_->getStamp());
}
else
{
imuT = imuIterB->second;
stampDiff = fabs(imuIterB->first - lastFrame_->getStamp());
}
}
return imuT;
}
} // namespace rtabmap