diff --git a/corelib/src/Graph.cpp b/corelib/src/Graph.cpp index c7c0979c21..2ec0bc5355 100644 --- a/corelib/src/Graph.cpp +++ b/corelib/src/Graph.cpp @@ -85,11 +85,12 @@ bool exportPoses( tmpPath+=".txt"; } - if(format == 1) + if(format == 1 || format == 10) { if(stamps.size() != poses.size()) { - UERROR("When exporting poses to format 1 (RGBD-SLAM), stamps and poses maps should have the same size!"); + UERROR("When exporting poses to format 1 (RGBD-SLAM), stamps and poses maps should have the same size! stamps=%d psoes=%d", + (int)stamps.size(), (int)poses.size()); return false; } } diff --git a/tools/Report/main.cpp b/tools/Report/main.cpp index e62ec7f508..f935ed8b81 100644 --- a/tools/Report/main.cpp +++ b/tools/Report/main.cpp @@ -183,6 +183,7 @@ int main(int argc, char * argv[]) driver->getAllNodeIds(ids); std::map, double> > stats = driver->getAllStatistics(); std::map odomPoses, gtPoses; + std::map odomStamps; std::vector cameraTime; cameraTime.reserve(ids.size()); std::vector odomTime; @@ -212,6 +213,7 @@ int main(int argc, char * argv[]) if(driver->getNodeInfo(*iter, p, m, w, l, s, gt, v, gps, sensors)) { odomPoses.insert(std::make_pair(*iter, p)); + odomStamps.insert(std::make_pair(*iter, s)); if(!gt.isNull()) { gtPoses.insert(std::make_pair(*iter, gt)); @@ -493,49 +495,55 @@ int main(int argc, char * argv[]) std::string dir = UDirectory::getDir(filePath); std::string dbName = UFile::getName(filePath); dbName = dbName.substr(0, dbName.size()-3); // remove db - std::string path = dir+UDirectory::separator()+dbName+"_poses.txt"; - if(!graph::exportPoses(path, outputKittiError?2:0, poses)) + std::string path = dir+UDirectory::separator()+dbName+"_slam.txt"; + std::multimap dummyLinks; + std::map stamps; + if(!outputKittiError) + { + for(std::map::iterator iter=poses.begin(); iter!=poses.end(); ++iter) + { + UASSERT(odomStamps.find(iter->first) != odomStamps.end()); + stamps.insert(*odomStamps.find(iter->first)); + } + } + if(!graph::exportPoses(path, outputKittiError?2:10, poses, dummyLinks, stamps)) { printf("Could not export the poses to \"%s\"!?!\n", path.c_str()); } + + //export odom + path = dir+UDirectory::separator()+dbName+"_odom.txt"; + stamps.clear(); + if(!outputKittiError) + { + for(std::map::iterator iter=odomPoses.begin(); iter!=odomPoses.end(); ++iter) + { + UASSERT(odomStamps.find(iter->first) != odomStamps.end()); + stamps.insert(*odomStamps.find(iter->first)); + } + } + if(!graph::exportPoses(path, outputKittiError?2:10, odomPoses, dummyLinks, stamps)) + { + printf("Could not export the ground truth to \"%s\"!?!\n", path.c_str()); + } + + //export ground truth if(groundTruth.size()) { - // For missing ground truth poses, set them to null - std::vector validIndices(poses.size(), 1); - int i=0; - for(std::map::iterator iter=poses.begin(); iter!=poses.end(); ++iter, ++i) + path = dir+UDirectory::separator()+dbName+"_gt.txt"; + stamps.clear(); + if(!outputKittiError) { - if(groundTruth.find(iter->first) == groundTruth.end()) + for(std::map::iterator iter=groundTruth.begin(); iter!=groundTruth.end(); ++iter) { - groundTruth.insert(std::make_pair(iter->first, Transform())); - validIndices[i] = 0; + UASSERT(odomStamps.find(iter->first) != odomStamps.end()); + stamps.insert(*odomStamps.find(iter->first)); } } - path = dir+UDirectory::separator()+dbName+"_gt.txt"; - if(!graph::exportPoses(path, outputKittiError?2:0, groundTruth)) + if(!graph::exportPoses(path, outputKittiError?2:10, groundTruth, dummyLinks, stamps)) { printf("Could not export the ground truth to \"%s\"!?!\n", path.c_str()); } - else - { - // save valid indices - path = dir+UDirectory::separator()+dbName+"_indices.txt"; - FILE * file = 0; - #ifdef _MSC_VER - fopen_s(&file, path.c_str(), "w"); - #else - file = fopen(path.c_str(), "w"); - #endif - if(file) - { - // VERTEX3 id x y z phi theta psi - for(unsigned int k=0; k