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

fix build errors with opencv2, added Transform::getClosestTransform() function for convenience.

Browse Source
This commit is contained in:
matlabbe
2019-05-31 16:51:37 -04:00
parent d5d00fbd7d
commit 4c8af6d6e9
7 changed files with 51 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -335,8 +335,7 @@ Transform OdometryF2M::computeTransform(
Transform imuT;
if(!imus_.empty())
{
double stampDiff = 0.0;
imuT = getClosestIMU(lastFrame_->getStamp(), stampDiff);
imuT = Transform::getClosestTransform(imus_, lastFrame_->getStamp());
}
// local bundle adjustment
@@ -1304,43 +1303,4 @@ 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());
}
//interpolate:
imuT = imuIterA->second.interpolate((stamp-imuIterA->first) / (imuIterB->first-imuIterA->first), imuIterB->second);
}
return imuT;
}
} // namespace rtabmap