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

OdometryF2M: refactored imu buffer

Browse Source
This commit is contained in:
matlabbe
2020-05-31 14:33:20 -04:00
parent caffb56246
commit c7be9a23ee
4 changed files with 43 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
corelib/include/rtabmap/core/Odometry.h
View File

@@ -78,6 +78,9 @@ public:
unsigned int framesProcessed() const {return framesProcessed_;}
bool imagesAlreadyRectified() const {return _imagesAlreadyRectified;}
protected:
const std::map<double, Transform> & imus() const {return imus_;}
private:
virtual Transform computeTransform(SensorData & data, const Transform & guess = Transform(), OdometryInfo * info = 0) = 0;
@@ -117,6 +120,7 @@ private:
std::vector<ParticleFilter *> particleFilters_;
cv::KalmanFilter kalmanFilter_;
StereoCameraModel stereoModel_;
std::map<double, Transform> imus_;
protected:
Odometry(const rtabmap::ParametersMap & parameters);

1
corelib/include/rtabmap/core/odometry/OdometryF2M.h
View File

@@ -79,7 +79,6 @@ private:
Signature * lastFrame_;
int lastFrameOldestNewId_;
std::vector<std::pair<pcl::PointCloud<pcl::PointNormal>::Ptr, pcl::IndicesPtr> > scansBuffer_;
std::map<double, Transform> imus_;
bool initGravity_;
std::map<int, std::map<int, FeatureBA> > bundleWordReferences_; //<WordId, <FrameId, pt2D+depth>>

38
corelib/src/Odometry.cpp
View File

@@ -186,7 +186,6 @@ Odometry::~Odometry()
{
delete particleFilters_[i];
}
particleFilters_.clear();
}
void Odometry::reset(const Transform & initialPose)
@@ -200,6 +199,7 @@ void Odometry::reset(const Transform & initialPose)
distanceTravelled_ = 0;
framesProcessed_ = 0;
imuLastTransform_.setNull();
imus_.clear();
if(_force3DoF || particleFilters_.size())
{
float x,y,z, roll,pitch,yaw;
@@ -383,6 +383,21 @@ 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();
@@ -429,22 +444,17 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
{
guess = guessIn;
}
else if(!data.imu().empty())
else if(!data.imu().empty() && !imus_.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))
imuCurrentTransform = Transform::getTransform(imus_, data.stamp());
if(!imuCurrentTransform.isNull() && !imuLastTransform_.isNull())
{
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());
}
Transform 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());
}
}

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

@@ -151,13 +151,6 @@ OdometryF2M::~OdometryF2M()
{
delete map_;
delete lastFrame_;
scansBuffer_.clear();
bundleWordReferences_.clear();
bundlePoses_.clear();
bundleLinks_.clear();
bundleModels_.clear();
bundlePoseReferences_.clear();
imus_.clear();
delete sba_;
delete regPipeline_;
UDEBUG("");
@@ -181,7 +174,6 @@ void OdometryF2M::reset(const Transform & initialPose)
bundlePoseReferences_.clear();
bundleSeq_ = 0;
lastFrameOldestNewId_ = 0;
imus_.clear();
}
initGravity_ = false;
}
@@ -206,30 +198,27 @@ Transform OdometryF2M::computeTransform(
info->type = 0;
}
Transform imuT;
if(sba_ && sba_->gravitySigma() > 0.0f && !data.imu().empty())
{
if(data.imu().orientation()[0] == 0.0 && data.imu().orientation()[1] == 0.0 && data.imu().orientation()[2] == 0.0)
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
{
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());
}
imuT = Transform::getTransform(imus(), data.stamp());
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(!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);
}
}
}
@@ -369,14 +358,9 @@ Transform OdometryF2M::computeTransform(
bundleLinks.insert(std::make_pair(bundlePoses_.rbegin()->first, Link(bundlePoses_.rbegin()->first, lastFrame_->id(), Link::kNeighbor, bundlePoses_.rbegin()->second.inverse()*transform, regInfo.covariance.inv())));
bundlePoses.insert(std::make_pair(lastFrame_->id(), transform));
Transform imuT;
if(!imus_.empty())
if(!imuT.isNull())
{
imuT = Transform::getTransform(imus_, lastFrame_->getStamp());
if(!imuT.isNull())
{
bundleLinks.insert(std::make_pair(lastFrame_->id(), Link(lastFrame_->id(), lastFrame_->id(), Link::kGravity, imuT)));
}
bundleLinks.insert(std::make_pair(lastFrame_->id(), Link(lastFrame_->id(), lastFrame_->id(), Link::kGravity, imuT)));
}
CameraModel model;
@@ -1256,7 +1240,7 @@ Transform OdometryF2M::computeTransform(
bundleModels_.insert(std::make_pair(lastFrame_->id(), model));
bundlePoses_.insert(std::make_pair(lastFrame_->id(), newFramePose));
if(!imus_.empty())
if(!imuT.isNull())
{
bundleIMUOrientations_.insert(std::make_pair(lastFrame_->id(), Link(lastFrame_->id(), lastFrame_->id(), Link::kGravity, newFramePose)));
}