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) 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) for(unsigned int i=0; i<info->newCorners.size(); ++i)
{ {
info->refCorners[i].x *= decimationRgb; info->refCorners[i].x *= decimationRgb;
info->refCorners[i].y *= decimationRgb; info->refCorners[i].y *= decimationRgb;
info->newCorners[i].x *= decimationRgb; if(!info->refCorners.empty())
info->newCorners[i].y *= decimationRgb; {
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) 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!"); UWARN("Odometry automatically reset to latest pose!");
this->reset(_pose); this->reset(_pose);
if(info)
{
*info = OdometryInfo();
}
return this->computeTransform(data, Transform(), info);
} }
} }