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

Odom: when odom is auto reset, reprocess the latest image and return its pose

Browse Source
This commit is contained in:
matlabbe
2021-06-24 13:41:38 -04:00
parent 5a676e9032
commit f5ab422c42
1 changed files with 11 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
corelib/src/Odometry.cpp
View File

@@ -623,13 +623,16 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
if(info)
{
UASSERT(info->newCorners.size() == info->refCorners.size());
UASSERT(info->newCorners.size() == info->refCorners.size() || info->refCorners.empty());
for(unsigned int i=0; i<info->newCorners.size(); ++i)
{
info->refCorners[i].x *= decimationRgb;
info->refCorners[i].y *= decimationRgb;
info->newCorners[i].x *= decimationRgb;
info->newCorners[i].y *= decimationRgb;
if(!info->refCorners.empty())
{
info->newCorners[i].x *= decimationRgb;
info->newCorners[i].y *= decimationRgb;
}
}
for(std::multimap<int, cv::KeyPoint>::iterator iter=info->words.begin(); iter!=info->words.end(); ++iter)
{
@@ -849,6 +852,11 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
{
UWARN("Odometry automatically reset to latest pose!");
this->reset(_pose);
if(info)
{
*info = OdometryInfo();
}
return this->computeTransform(data, Transform(), info);
}
}