i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

Deprecated getClosestTransform(), use getTransform() instead. OdometryF2M: removed patch about forcing roll/pitch from IMU (BA is already doing the job).

Browse Source
This commit is contained in:
matlabbe
2019-09-15 17:31:58 -04:00
parent 350d3cd85f
commit b2b1977f8b
3 changed files with 50 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

37
corelib/src/Transform.cpp
View File

@@ -467,6 +467,42 @@ bool Transform::canParseString(const std::string & string)
return list.size() == 0 || list.size() == 3 || list.size() == 6 || list.size() == 7 || list.size() == 9 || list.size() == 12;
}
Transform Transform::getTransform(
const std::map<double, Transform> & tfBuffer,
const double & stamp)
{
UASSERT(!tfBuffer.empty());
std::map<double, Transform>::const_iterator imuIterB = tfBuffer.lower_bound(stamp);
std::map<double, Transform>::const_iterator imuIterA = imuIterB;
if(imuIterA != tfBuffer.begin())
{
imuIterA = --imuIterA;
}
if(imuIterB == tfBuffer.end())
{
imuIterB = --imuIterB;
}
Transform imuT;
if(imuIterB->first == stamp)
{
imuT = imuIterB->second;
}
else if(imuIterA != imuIterB)
{
//interpolate:
imuT = imuIterA->second.interpolate((stamp-imuIterA->first) / (imuIterB->first-imuIterA->first), imuIterB->second);
}
else if(stamp > imuIterB->first)
{
UWARN("No transform found for stamp %f! Latest is %f", stamp, imuIterB->first);
}
else
{
UWARN("No transform found for stamp %f! Earliest is %f", stamp, imuIterA->first);
}
return imuT;
}
Transform Transform::getClosestTransform(
const std::map<double, Transform> & tfBuffer,
const double & stamp,
@@ -504,4 +540,5 @@ Transform Transform::getClosestTransform(
return imuT;
}
}

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

@@ -333,22 +333,6 @@ Transform OdometryF2M::computeTransform(
UDEBUG("Registration time = %fs", regInfo.totalTime);
if(!transform.isNull())
{
cv::Mat var = regInfo.covariance.clone();
Transform imuT;
if(!imus_.empty())
{
imuT = Transform::getClosestTransform(imus_, lastFrame_->getStamp());
if(!imuT.isNull())
{
float roll, pitch, yaw;
imuT.getEulerAngles(roll, pitch, yaw);
transform = Transform(transform.x(),transform.y(),transform.z(),roll,pitch,transform.theta());
var.at<double>(3,3)/=100.0; // roll
var.at<double>(4,4)/=100.0; // pitch
}
}
// local bundle adjustment
if(bundleAdjustment_>0 && sba_ &&
regPipeline_->isImageRequired() &&
@@ -379,12 +363,17 @@ Transform OdometryF2M::computeTransform(
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());
bundleLinks.insert(std::make_pair(bundlePoses_.rbegin()->first, Link(bundlePoses_.rbegin()->first, lastFrame_->id(), Link::kNeighbor, bundlePoses_.rbegin()->second.inverse()*transform, var.inv())));
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));
if(!imuT.isNull())
Transform imuT;
if(!imus_.empty())
{
bundleLinks.insert(std::make_pair(lastFrame_->id(), Link(lastFrame_->id(), lastFrame_->id(), Link::kGravity, imuT)));
imuT = Transform::getTransform(imus_, lastFrame_->getStamp());
if(!imuT.isNull())
{
bundleLinks.insert(std::make_pair(lastFrame_->id(), Link(lastFrame_->id(), lastFrame_->id(), Link::kGravity, imuT)));
}
}
CameraModel model;