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

Added Ground truth import GPS format. Alignement between estimated and ground truth poses is done with SVD.

Browse Source
This commit is contained in:
matlabbe
2016-01-15 17:05:55 -05:00
parent 2d6965aa4b
commit 35b722ba08
9 changed files with 175 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files
guilib/src/MainWindow.cpp
+31 -31
View File
@@ -2134,45 +2134,45 @@ Transform MainWindow::alignPosesToGroundTruth(
std::map<int, Transform> & poses,
const std::map<int, Transform> & groundTruth)
{
UDEBUG("");
Transform t = Transform::getIdentity();
if(groundTruth.size())
if(groundTruth.size() && poses.size())
{
if(uContains(_preferencesDialog->getAllParameters(), Parameters::kRGBDOptimizeFromGraphEnd()))
unsigned int maxSize = poses.size()>groundTruth.size()?poses.size():groundTruth.size();
pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
cloud1.resize(maxSize);
cloud2.resize(maxSize);
int oi = 0;
int idFirst = 0;
for(std::map<int, Transform>::const_iterator iter=groundTruth.begin(); iter!=groundTruth.end(); ++iter)
{
bool optimizeFromGraphEnd = uStr2Bool(_preferencesDialog->getAllParameters().at(Parameters::kRGBDOptimizeFromGraphEnd()));
// Align poses to ground truth
int rootId = 0;
if(!optimizeFromGraphEnd)
std::map<int, Transform>::iterator iter2 = poses.find(iter->first);
if(iter2!=poses.end())
{
for(std::map<int, Transform>::const_iterator iter=groundTruth.begin(); iter!=groundTruth.end(); ++iter)
if(oi==0)
{
std::map<int, Transform>::iterator iter2 = poses.find(iter->first);
if(iter2!=poses.end())
{
rootId = iter->first;
break;
}
idFirst = iter->first;
}
cloud1[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
cloud2[oi++] = pcl::PointXYZ(iter2->second.x(), iter2->second.y(), iter2->second.z());
}
else
}
if(oi>1)
{
cloud1.resize(oi);
cloud2.resize(oi);
t = util3d::transformFromXYZCorrespondencesSVD(cloud2, cloud1);
}
else if(oi==1)
{
t = groundTruth.at(idFirst) * poses.at(idFirst).inverse();
}
if(!t.isIdentity())
{
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
for(std::map<int, Transform>::const_reverse_iterator iter=groundTruth.rbegin(); iter!=groundTruth.rend(); ++iter)
{
std::map<int, Transform>::iterator iter2 = poses.find(iter->first);
if(iter2!=poses.end())
{
rootId = iter->first;
break;
}
}
}
if(rootId>0)
{
t = groundTruth.at(rootId) * poses.at(rootId).inverse();
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
iter->second = t * iter->second;
}
iter->second = t * iter->second;
}
}
UDEBUG("t=%s", t.prettyPrint().c_str());