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

Export Poses: Added support of landmarks. Fixed g2o export with 6DoF landmark constaints. Adjusted g2o export landmark id. (#1199)

Browse Source
This commit is contained in:
matlabbe
2024-01-20 12:04:26 -08:00
parent 856e372dde
commit 13cd5e7a5e
3 changed files with 211 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files

45
guilib/src/DatabaseViewer.cpp
View File

@@ -2753,14 +2753,24 @@ void DatabaseViewer::exportPoses(int format)
}
}
if(format != 4 && !poses.empty() && poses.begin()->first<0) // not g2o, landmark not supported
if(format != 4 && format != 11 && !poses.empty() && poses.begin()->first<0) // not g2o, landmark not supported
{
UWARN("Only g2o format (4) can export landmarks, they are ignored with format %d", format);
std::map<int, Transform>::iterator iter=poses.begin();
while(iter!=poses.end() && iter->first < 0)
UWARN("Only g2o format (4) and RGBD format with ID format can export landmarks, they are ignored with format %d", format);
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end() && iter->first < 0;)
{
poses.erase(iter++);
}
for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end();)
{
if(iter->second.from() < 0 || iter->second.to() < 0)
{
links.erase(iter++);
}
else
{
++iter;
}
}
}
std::map<int, double> stamps;
@@ -2768,17 +2778,24 @@ void DatabaseViewer::exportPoses(int format)
{
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
Transform p, g;
int w;
std::string l;
double stamp=0.0;
int mapId;
std::vector<float> v;
GPS gps;
EnvSensors sensors;
if(dbDriver_->getNodeInfo(iter->first, p, mapId, w, l, stamp, g, v, gps, sensors))
if(iter->first<0 && format == 11) // in case of landmarks
{
stamps.insert(std::make_pair(iter->first, stamp));
stamps.insert(std::make_pair(iter->first, 0));
}
else
{
Transform p, g;
int w;
std::string l;
double stamp=0.0;
int mapId;
std::vector<float> v;
GPS gps;
EnvSensors sensors;
if(dbDriver_->getNodeInfo(iter->first, p, mapId, w, l, stamp, g, v, gps, sensors))
{
stamps.insert(std::make_pair(iter->first, stamp));
}
}
}
if(stamps.size()!=poses.size())

28
guilib/src/MainWindow.cpp
View File

@@ -6269,8 +6269,8 @@ void MainWindow::exportPoses(int format)
std::multimap<int, Link> links;
if(localTransforms.empty())
{
poses = std::map<int, Transform>(_currentPosesMap.lower_bound(1), _currentPosesMap.end());
links = std::multimap<int, Link>(_currentLinksMap.lower_bound(1), _currentLinksMap.end());
poses = std::map<int, Transform>(_currentPosesMap.begin(), _currentPosesMap.end());
links = std::multimap<int, Link>(_currentLinksMap.begin(), _currentLinksMap.end());
}
else
{
@@ -6291,14 +6291,24 @@ void MainWindow::exportPoses(int format)
}
}
if(format != 4 && !poses.empty() && poses.begin()->first<0) // not g2o, landmark not supported
if(format != 4 && format != 11 && !poses.empty() && poses.begin()->first<0) // not g2o, landmark not supported
{
UWARN("Only g2o format (4) can export landmarks, they are ignored with format %d", format);
std::map<int, Transform>::iterator iter=poses.begin();
while(iter!=poses.end() && iter->first < 0)
UWARN("Only g2o format (4) and RGBD format with ID format can export landmarks, they are ignored with format %d", format);
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end() && iter->first < 0;)
{
poses.erase(iter++);
}
for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end();)
{
if(iter->second.from() < 0 || iter->second.to() < 0)
{
links.erase(iter++);
}
else
{
++iter;
}
}
}
std::map<int, double> stamps;
@@ -6306,7 +6316,11 @@ void MainWindow::exportPoses(int format)
{
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
if(_cachedSignatures.contains(iter->first))
if(iter->first < 0 && format == 11) // in case of landmarks
{
stamps.insert(std::make_pair(iter->first, 0));
}
else if(_cachedSignatures.contains(iter->first))
{
stamps.insert(std::make_pair(iter->first, _cachedSignatures.value(iter->first).getStamp()));
}