i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-09 21:10:19 +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
corelib/include/rtabmap/core/Graph.h
+1 -1
View File
@@ -53,7 +53,7 @@ bool RTABMAP_EXP exportPoses(
bool RTABMAP_EXP importPoses(
const std::string & filePath,
int format, // 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o
int format, // 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, GPS (t,x,y)
std::map<int, Transform> & poses,
std::multimap<int, Link> * constraints = 0, // optional for formats 3 and 4
std::map<int, double> * stamps = 0); // optional for format 1
corelib/include/rtabmap/core/util3d_registration.h
+4
View File
@@ -45,6 +45,10 @@ int RTABMAP_EXP getCorrespondencesCount(const pcl::PointCloud<pcl::PointXYZ>::Co
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
float maxDistance);
Transform RTABMAP_EXP transformFromXYZCorrespondencesSVD(
const pcl::PointCloud<pcl::PointXYZ> & cloud1,
const pcl::PointCloud<pcl::PointXYZ> & cloud2);
Transform RTABMAP_EXP transformFromXYZCorrespondences(
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud1,
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud2,
corelib/src/CameraRGB.cpp
+29 -11
View File
@@ -293,19 +293,19 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
UERROR("Cannot read ground truth file \"%s\".", groundTruthPath_.c_str());
success = false;
}
else if(_groundTruthFormat != 1 && poses.size() != this->imagesCount())
else if((_groundTruthFormat != 1 && _groundTruthFormat != 5) && poses.size() != this->imagesCount())
{
UERROR("The ground truth count is not the same as the images (%d vs %d)! Please remove "
"the ground truth file path if you don't want to use it (current file path=%s).",
(int)poses.size(), this->imagesCount(), groundTruthPath_.c_str());
success = false;
}
else if(_groundTruthFormat == 1 && stamps_.size() == 0)
else if((_groundTruthFormat == 1 || _groundTruthFormat == 5) && stamps_.size() == 0)
{
UERROR("When using rgbd-slam format for ground truth, images must have timestamps!");
UERROR("When using RGBD-SLAM and GPS formats for ground truth, images must have timestamps!");
success = false;
}
else if(_groundTruthFormat == 1)
else if(_groundTruthFormat == 1 || _groundTruthFormat == 5)
{
UDEBUG("");
//Match ground truth values with images
@@ -317,10 +317,12 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
}
std::vector<double> values = uValues(stamps);
int validPoses = 0;
for(std::list<double>::iterator ster=stamps_.begin(); ster!=stamps_.end(); ++ster)
{
Transform pose; // null transform
std::map<double, int>::iterator endIter = stampsToIds.lower_bound(*ster);
bool warned = false;
if(endIter != stampsToIds.end())
{
if(endIter->first == *ster)
@@ -335,21 +337,37 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
double stampBeg = beginIter->first;
double stampEnd = endIter->first;
UASSERT(stampEnd > stampBeg && *ster>stampBeg && *ster < stampEnd);
float t = (*ster - stampBeg) / (stampEnd-stampBeg);
Transform & ta = poses.at(beginIter->second);
Transform & tb = poses.at(endIter->second);
if(!ta.isNull() && !tb.isNull())
if(stampEnd - stampBeg > 10.0)
{
pose = ta.interpolate(t, tb);
warned = true;
UDEBUG("Cannot interpolate ground truth pose for stamp %f between %f and %f (>10 sec)",
*ster,
stampBeg,
stampEnd);
}
else
{
float t = (*ster - stampBeg) / (stampEnd-stampBeg);
Transform & ta = poses.at(beginIter->second);
Transform & tb = poses.at(endIter->second);
if(!ta.isNull() && !tb.isNull())
{
++validPoses;
pose = ta.interpolate(t, tb);
}
}
}
}
if(pose.isNull())
if(pose.isNull() && !warned)
{
UWARN("Ground truth pose not found for stamp %f", *ster);
UDEBUG("Ground truth pose not found for stamp %f", *ster);
}
groundTruth_.push_back(pose);
}
if(validPoses != (int)stamps.size())
{
UWARN("%d valid ground truth poses of %d stamps", validPoses, (int)stamps_.size());
}
}
else
{
corelib/src/Graph.cpp
+60 -15
View File
@@ -153,7 +153,7 @@ bool exportPoses(
bool importPoses(
const std::string & filePath,
int format, // 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o
int format, // 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=GPS (t,x,y)
std::map<int, Transform> & poses,
std::multimap<int, Link> * constraints, // optional for formats 3 and 4
std::map<int, double> * stamps) // optional for format 1
@@ -197,7 +197,49 @@ bool importPoses(
continue;
}
if(format == 1) // rgbd-slam format
if(format == 5) // GPS format
{
std::vector<std::string> strList = uListToVector(uSplit(str));
if(strList.size() == 3 || strList.size() == 4)
{
if( uIsNumber(uReplaceChar(strList[0], ' ', "")) &&
uIsNumber(uReplaceChar(strList[1], ' ', "")) &&
uIsNumber(uReplaceChar(strList[2], ' ', "")) &&
(strList.size()==3 || uIsNumber(uReplaceChar(strList[3], ' ', ""))))
{
double stamp = uStr2Double(uReplaceChar(strList[0], ' ', ""));
double x = uStr2Double(uReplaceChar(strList[1], ' ', ""));
double y = uStr2Double(uReplaceChar(strList[2], ' ', ""));
if(stamps)
{
stamps->insert(std::make_pair(id, stamp));
}
float yaw = 0.0f;
if(strList.size()==4)
{
yaw = uStr2Double(uReplaceChar(strList[3], ' ', ""));
}
else if(uContains(poses, id-1))
{
// set yaw depending on successive poses
Transform & previousPose = poses.at(id-1);
yaw = atan2(y-previousPose.y(),x-previousPose.x());
previousPose = Transform(previousPose.x(), previousPose.y(), yaw);
}
poses.insert(std::make_pair(id, Transform(x,y,0,0,0,yaw)));
}
else
{
UDEBUG("Not valid values detected: \"%s\"", str.c_str());
}
}
else
{
UERROR("Error parsing \"%s\" with GPS format (should have 3 values: stamp x y)", str.c_str());
}
}
else if(format == 1) // rgbd-slam format
{
std::list<std::string> strList = uSplit(str);
if(strList.size() == 8)
@@ -210,21 +252,24 @@ bool importPoses(
{
UWARN("Null transform read!? line parsed: \"%s\"", str.c_str());
}
if(stamps)
else
{
stamps->insert(std::make_pair(id, stamp));
if(stamps)
{
stamps->insert(std::make_pair(id, stamp));
}
// we need to remove optical rotation
// z pointing front, x left, y down
Transform t( 0, 0, 1, 0,
-1, 0, 0, 0,
0,-1, 0, 0);
pose = t * pose * t.inverse();
t = Transform( 0, 0, 1, 0,
0, -1, 0, 0,
1, 0, 0, 0);
pose = t*pose;
poses.insert(std::make_pair(id, pose));
}
// we need to remove optical rotation
// z pointing front, x left, y down
Transform t( 0, 0, 1, 0,
-1, 0, 0, 0,
0,-1, 0, 0);
pose = t * pose * t.inverse();
t = Transform( 0, 0, 1, 0,
0, -1, 0, 0,
1, 0, 0, 0);
pose = t*pose;
poses.insert(std::make_pair(id, pose));
}
else
{
corelib/src/util3d_registration.cpp
+14
View File
@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/registration/icp.h>
#include <pcl/registration/transformation_estimation_2D.h>
#include <pcl/registration/transformation_estimation_svd.h>
#include <pcl/sample_consensus/sac_model_registration.h>
#include <pcl/sample_consensus/ransac.h>
#include <rtabmap/utilite/ULogger.h>
@@ -43,6 +44,19 @@ namespace rtabmap
namespace util3d
{
// Get transform from cloud2 to cloud1
Transform transformFromXYZCorrespondencesSVD(
const pcl::PointCloud<pcl::PointXYZ> & cloud1,
const pcl::PointCloud<pcl::PointXYZ> & cloud2)
{
pcl::registration::TransformationEstimationSVD<pcl::PointXYZ, pcl::PointXYZ> svd;
// Perform the alignment
Eigen::Matrix4f matrix;
svd.estimateRigidTransformation(cloud1, cloud2, matrix);
return Transform::fromEigen4f(matrix);
}
// Get transform from cloud2 to cloud1
Transform transformFromXYZCorrespondences(
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud1,
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());
guilib/src/PreferencesDialog.cpp
+1
View File
@@ -164,6 +164,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->reextract_type->setItemData(6, 0, Qt::UserRole - 1);
#endif
}
_ui->comboBox_cameraImages_gtFormat->setItemData(4, 0, Qt::UserRole - 1);
if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
{
_ui->graphOptimization_type->setItemData(1, 0, Qt::UserRole - 1);
guilib/src/ui/preferencesDialog.ui
+13 -3
View File
@@ -63,9 +63,9 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-946</y>
<width>676</width>
<height>1919</height>
<y>-499</y>
<width>680</width>
<height>1171</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_16">
@@ -3434,6 +3434,16 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<string>TORO</string>
</property>
</item>
<item>
<property name="text">
<string>g2o</string>
</property>
</item>
<item>
<property name="text">
<string>GPS</string>
</property>
</item>
</widget>
</item>
<item row="4" column="2">
utilite/include/rtabmap/utilite/UStl.h
+22 -2
View File
@@ -625,7 +625,7 @@ inline bool uIsDigit(const char c)
}
/**
* Check if a string is a interger number.
* Check if a string is a integer number.
* @param str the string
* @return true if the string is a integer number
*/
@@ -634,11 +634,31 @@ inline bool uIsInteger(const std::string & str)
bool isInteger = str.size()!=0;
for(unsigned int i=0; i<str.size() && isInteger; ++i)
{
isInteger = uIsDigit(str[i]);
isInteger = i==0&&str[i]=='-'||uIsDigit(str[i]);
}
return isInteger;
}
/**
* Check if a string is a number (integer or float).
* @param str the string
* @return true if the string is a number
*/
inline bool uIsNumber(const std::string & str)
{
std::list<std::string> list = uSplit(str, '.');
if(list.size() == 1)
{
return uIsInteger(str);
}
else if(list.size() == 2)
{
return uIsInteger(list.front()) && uIsInteger(list.back());
}
return false;
}
/**
* Split a string into number and character strings.
* Example: