i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-04 09:07:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added Transform and VisualWord tests

Browse Source
This commit is contained in:
matlabbe
2025-07-06 13:23:13 -07:00
parent b8d253129c
commit d30e76bc7a
7 changed files with 590 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Transform.cpp
+15 -15
View File
@@ -530,35 +530,35 @@ Transform Transform::getTransform(
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())
std::map<double, Transform>::const_iterator iterB = tfBuffer.lower_bound(stamp);
std::map<double, Transform>::const_iterator iterA = iterB;
if(iterA != tfBuffer.begin())
{
imuIterA = --imuIterA;
iterA = --iterA;
}
if(imuIterB == tfBuffer.end())
if(iterB == tfBuffer.end())
{
imuIterB = --imuIterB;
iterB = --iterB;
}
Transform imuT;
if(imuIterB->first == stamp)
Transform t;
if(iterB->first == stamp)
{
imuT = imuIterB->second;
t = iterB->second;
}
else if(imuIterA != imuIterB)
else if(iterA != iterB)
{
//interpolate:
imuT = imuIterA->second.interpolate((stamp-imuIterA->first) / (imuIterB->first-imuIterA->first), imuIterB->second);
t = iterA->second.interpolate((stamp-iterA->first) / (iterB->first-iterA->first), iterB->second);
}
else if(stamp > imuIterB->first)
else if(stamp > iterB->first)
{
UWARN("No transform found for stamp %f! Latest is %f", stamp, imuIterB->first);
UWARN("No transform found for stamp %f! Latest is %f", stamp, iterB->first);
}
else
{
UWARN("No transform found for stamp %f! Earliest is %f", stamp, imuIterA->first);
UWARN("No transform found for stamp %f! Earliest is %f", stamp, iterA->first);
}
return imuT;
return t;
}
Transform Transform::getClosestTransform(
corelib/src/VisualWord.cpp
-1
View File
@@ -73,7 +73,6 @@ unsigned long VisualWord::getMemoryUsed() const
{
unsigned long memoryUsage = sizeof(VisualWord);
memoryUsage += _references.size() * (sizeof(int)*2+sizeof(std::map<int ,int>::iterator)) + sizeof(std::map<int ,int>);
memoryUsage += _oldReferences.size() * (sizeof(int)*2+sizeof(std::map<int ,int>::iterator)) + sizeof(std::map<int ,int>);
memoryUsage += _descriptor.total() * _descriptor.elemSize();
return memoryUsage;
}