i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added ground truth support of Karlsruhe dataset

Browse Source
This commit is contained in:
matlabbe
2016-01-19 21:53:45 -05:00
parent eb21280f04
commit c367fe915e
3 changed files with 50 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
corelib/src/CameraRGB.cpp
View File

@@ -391,6 +391,14 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
{
UDEBUG("");
groundTruth_ = uValuesList(poses);
if(stamps_.size() == 0 && stamps.size() == poses.size())
{
stamps_ = uValuesList(stamps);
}
else if(_groundTruthFormat==8 && stamps_.size() == 0 && stamps.size()>0 && stamps.size() != poses.size())
{
UERROR("With Karlsruhe ground truth format, timestamps (%d) and poses (%d) should match!", (int)stamps.size(), (int)poses.size());
}
}
UASSERT_MSG(groundTruth_.size() == stamps_.size(), uFormat("%d vs %d", (int)groundTruth_.size(), (int)stamps_.size()).c_str());
}

38
corelib/src/Graph.cpp
View File

@@ -154,12 +154,12 @@ bool exportPoses(
bool importPoses(
const std::string & filePath,
int format, // 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS
int format, // 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe
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
{
UDEBUG("%s", filePath.c_str());
UDEBUG("%s format=%d", filePath.c_str(), format);
if(format==3) // TORO
{
std::multimap<int, Link> constraintsTmp;
@@ -200,7 +200,39 @@ bool importPoses(
continue;
}
if(format == 7) // St Lucia format
if(format == 8) // Karlsruhe format
{
std::vector<std::string> strList = uListToVector(uSplit(str));
if(strList.size() == 10)
{
// timestamp,lat,lon,alt,x,y,z,roll,pitch,yaw
// stamp is 1252400096825289728, transform to 125240009.6825289728
double stamp = uStr2Double(strList[0].insert(10, 1, '.'));
double x = uStr2Double(strList[4]);
double y = uStr2Double(strList[5]);
double z = uStr2Double(strList[6]);
double roll = uStr2Double(strList[8]);
double pitch = uStr2Double(strList[7]);
double yaw = -(uStr2Double(strList[9])-M_PI_2);
if(stamps)
{
stamps->insert(std::make_pair(id, stamp));
}
Transform pose = Transform(x,y,z,roll,pitch,yaw);
if(poses.size() == 0)
{
originPose = pose.inverse();
}
pose = originPose * pose; // transform in local coordinate where first value is the origin
poses.insert(std::make_pair(id, pose));
}
else
{
UERROR("Error parsing \"%s\" with Karlsruhe format (should have 10 values)", str.c_str());
}
}
else if(format == 7) // St Lucia format
{
std::vector<std::string> strList = uListToVector(uSplit(str));
if(strList.size() == 12)