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

Implemented OptimizerG2O::loadGraph()

Browse Source
This commit is contained in:
matlabbe
2026-05-21 18:41:40 -07:00
parent 82f2edac96
commit dc21e6df76
4 changed files with 407 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/optimizer/OptimizerG2O.h
+6
View File
@@ -41,6 +41,12 @@ public:
static bool isCSparseAvailable(); static bool isCSparseAvailable();
static bool isCholmodAvailable(); static bool isCholmodAvailable();
public:
static bool loadGraph(
const std::string & fileName,
std::map<int, Transform> & poses,
std::multimap<int, Link> & edgeConstraints);
public: public:
OptimizerG2O(const ParametersMap & parameters = ParametersMap()); OptimizerG2O(const ParametersMap & parameters = ParametersMap());
virtual ~OptimizerG2O() {} virtual ~OptimizerG2O() {}
corelib/src/Graph.cpp
+8 -1
View File
@@ -218,7 +218,14 @@ bool importPoses(
else if(format == 4) // g2o else if(format == 4) // g2o
{ {
std::multimap<int, Link> constraintsTmp; std::multimap<int, Link> constraintsTmp;
UERROR("Cannot import from g2o format because it is not yet supported!"); if(OptimizerG2O::loadGraph(filePath, poses, constraintsTmp))
{
if(constraints)
{
*constraints = constraintsTmp;
}
return true;
}
return false; return false;
} }
else else
corelib/src/optimizer/OptimizerG2O.cpp
+384
View File
@@ -2135,6 +2135,390 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
return optimizedPoses; return optimizedPoses;
} }
bool OptimizerG2O::loadGraph(
const std::string & fileName,
std::map<int, Transform> & poses,
std::multimap<int, Link> & edgeConstraints)
{
FILE * file = 0;
#ifdef _MSC_VER
fopen_s(&file, fileName.c_str(), "r");
#else
file = fopen(fileName.c_str(), "r");
#endif
if(!file)
{
UERROR("Cannot open file %s", fileName.c_str());
return false;
}
// saveGraph() writes landmarks (originally negative ids, remapped to
// landmarkOffset - id) first in DESCENDING file-id order, then regular
// poses in ASCENDING file-id order. We recover landmarkOffset from this
// order to restore the original negative landmark ids.
struct VertexEntry {
int fileId;
Transform transform;
bool definitelyLandmark; // VERTEX_XY / VERTEX_TRACKXYZ
};
struct EdgeEntry {
int from;
int to;
Link::Type type;
Transform transform;
cv::Mat info;
bool isPrior; // from==to, prior on a single vertex
bool hasLandmarkEndpoint; // tag implies a landmark on one side
};
std::vector<VertexEntry> verticesList;
std::vector<EdgeEntry> edgesList;
char line[2048];
while(fgets(line, 2048, file) != NULL)
{
std::list<std::string> tokenList = uSplit(uReplaceChar(uReplaceChar(line, '\n', ' '), '\r', ' '), ' ');
std::vector<std::string> v;
v.reserve(tokenList.size());
for(std::list<std::string>::const_iterator iter = tokenList.begin(); iter != tokenList.end(); ++iter)
{
if(!iter->empty())
{
v.push_back(*iter);
}
}
if(v.empty())
{
continue;
}
const std::string & tag = v[0];
// Skip parameters, switch helpers and unrelated entries
if(tag == "PARAMS_SE2OFFSET" || tag == "PARAMS_SE3OFFSET" ||
tag == "VERTEX_SWITCH" || tag == "EDGE_SWITCH_PRIOR")
{
continue;
}
if(tag == "VERTEX_SE2" && v.size() == 5)
{
VertexEntry e;
e.fileId = atoi(v[1].c_str());
e.transform = Transform(uStr2Float(v[2]), uStr2Float(v[3]), uStr2Float(v[4]));
e.definitelyLandmark = false;
verticesList.push_back(e);
}
else if(tag == "VERTEX_XY" && v.size() == 4)
{
VertexEntry e;
e.fileId = atoi(v[1].c_str());
e.transform = Transform(uStr2Float(v[2]), uStr2Float(v[3]), 0);
e.definitelyLandmark = true;
verticesList.push_back(e);
}
else if(tag == "VERTEX_SE3:QUAT" && v.size() == 9)
{
VertexEntry e;
e.fileId = atoi(v[1].c_str());
e.transform = Transform(uStr2Float(v[2]), uStr2Float(v[3]), uStr2Float(v[4]),
uStr2Float(v[5]), uStr2Float(v[6]), uStr2Float(v[7]), uStr2Float(v[8]));
e.definitelyLandmark = false;
verticesList.push_back(e);
}
else if(tag == "VERTEX_TRACKXYZ" && v.size() == 5)
{
VertexEntry e;
e.fileId = atoi(v[1].c_str());
e.transform = Transform(uStr2Float(v[2]), uStr2Float(v[3]), uStr2Float(v[4]), 0, 0, 0);
e.definitelyLandmark = true;
verticesList.push_back(e);
}
else if(tag == "EDGE_SE2" && v.size() == 12)
{
EdgeEntry e;
e.from = atoi(v[1].c_str());
e.to = atoi(v[2].c_str());
e.transform = Transform(uStr2Float(v[3]), uStr2Float(v[4]), uStr2Float(v[5]));
e.info = cv::Mat::eye(6, 6, CV_64FC1);
e.info.at<double>(0, 0) = uStr2Double(v[6]);
e.info.at<double>(0, 1) = e.info.at<double>(1, 0) = uStr2Double(v[7]);
e.info.at<double>(0, 5) = e.info.at<double>(5, 0) = uStr2Double(v[8]);
e.info.at<double>(1, 1) = uStr2Double(v[9]);
e.info.at<double>(1, 5) = e.info.at<double>(5, 1) = uStr2Double(v[10]);
e.info.at<double>(5, 5) = uStr2Double(v[11]);
e.type = Link::kUndef; // disambiguated after we know landmarkOffset
e.isPrior = false;
e.hasLandmarkEndpoint = false;
edgesList.push_back(e);
}
else if(tag == "EDGE_SE2_XY" && v.size() == 8)
{
EdgeEntry e;
e.from = atoi(v[1].c_str());
e.to = atoi(v[2].c_str());
e.transform = Transform(uStr2Float(v[3]), uStr2Float(v[4]), 0);
e.info = cv::Mat::eye(6, 6, CV_64FC1);
e.info.at<double>(0, 0) = uStr2Double(v[5]);
e.info.at<double>(0, 1) = e.info.at<double>(1, 0) = uStr2Double(v[6]);
e.info.at<double>(1, 1) = uStr2Double(v[7]);
e.type = Link::kLandmark;
e.isPrior = false;
e.hasLandmarkEndpoint = true;
edgesList.push_back(e);
}
else if((tag == "EDGE_SE3:QUAT" || tag == "EDGE_SE3") && v.size() == 31)
{
EdgeEntry e;
e.from = atoi(v[1].c_str());
e.to = atoi(v[2].c_str());
e.transform = Transform(uStr2Float(v[3]), uStr2Float(v[4]), uStr2Float(v[5]),
uStr2Float(v[6]), uStr2Float(v[7]), uStr2Float(v[8]), uStr2Float(v[9]));
e.info = cv::Mat::eye(6, 6, CV_64FC1);
int idx = 10;
for(int r = 0; r < 6; ++r)
{
for(int c = r; c < 6; ++c)
{
e.info.at<double>(r, c) = uStr2Double(v[idx++]);
if(r != c) e.info.at<double>(c, r) = e.info.at<double>(r, c);
}
}
// EDGE_SE3 (no :QUAT) is the landmark variant emitted by saveGraph
bool landmarkTag = (tag == "EDGE_SE3");
e.type = landmarkTag ? Link::kLandmark : Link::kUndef;
e.isPrior = false;
e.hasLandmarkEndpoint = landmarkTag;
edgesList.push_back(e);
}
else if(tag == "EDGE_SE3_TRACKXYZ" && v.size() == 13)
{
EdgeEntry e;
e.from = atoi(v[1].c_str());
e.to = atoi(v[2].c_str());
// v[3] = param_offset id, ignored
e.transform = Transform(uStr2Float(v[4]), uStr2Float(v[5]), uStr2Float(v[6]), 0, 0, 0);
e.info = cv::Mat::eye(6, 6, CV_64FC1);
e.info.at<double>(0, 0) = uStr2Double(v[7]);
e.info.at<double>(0, 1) = e.info.at<double>(1, 0) = uStr2Double(v[8]);
e.info.at<double>(0, 2) = e.info.at<double>(2, 0) = uStr2Double(v[9]);
e.info.at<double>(1, 1) = uStr2Double(v[10]);
e.info.at<double>(1, 2) = e.info.at<double>(2, 1) = uStr2Double(v[11]);
e.info.at<double>(2, 2) = uStr2Double(v[12]);
e.type = Link::kLandmark;
e.isPrior = false;
e.hasLandmarkEndpoint = true;
edgesList.push_back(e);
}
else if(tag == "EDGE_PRIOR_SE2" && v.size() == 11)
{
EdgeEntry e;
e.from = atoi(v[1].c_str());
e.to = e.from;
e.transform = Transform(uStr2Float(v[2]), uStr2Float(v[3]), uStr2Float(v[4]));
e.info = cv::Mat::eye(6, 6, CV_64FC1);
e.info.at<double>(0, 0) = uStr2Double(v[5]);
e.info.at<double>(0, 1) = e.info.at<double>(1, 0) = uStr2Double(v[6]);
e.info.at<double>(0, 5) = e.info.at<double>(5, 0) = uStr2Double(v[7]);
e.info.at<double>(1, 1) = uStr2Double(v[8]);
e.info.at<double>(1, 5) = e.info.at<double>(5, 1) = uStr2Double(v[9]);
e.info.at<double>(5, 5) = uStr2Double(v[10]);
e.type = Link::kPosePrior;
e.isPrior = true;
e.hasLandmarkEndpoint = false;
edgesList.push_back(e);
}
else if(tag == "EDGE_PRIOR_SE2_XY" && v.size() == 7)
{
EdgeEntry e;
e.from = atoi(v[1].c_str());
e.to = e.from;
e.transform = Transform(uStr2Float(v[2]), uStr2Float(v[3]), 0);
e.info = cv::Mat::eye(6, 6, CV_64FC1);
e.info.at<double>(0, 0) = uStr2Double(v[4]);
e.info.at<double>(0, 1) = e.info.at<double>(1, 0) = uStr2Double(v[5]);
e.info.at<double>(1, 1) = uStr2Double(v[6]);
// no orientation info on this prior
e.info.at<double>(3, 3) = e.info.at<double>(4, 4) = e.info.at<double>(5, 5) = 1.0 / 9999.0;
e.type = Link::kPosePrior;
e.isPrior = true;
e.hasLandmarkEndpoint = false;
edgesList.push_back(e);
}
else if(tag == "EDGE_SE3_PRIOR" && v.size() == 31)
{
EdgeEntry e;
e.from = atoi(v[1].c_str());
e.to = e.from;
// v[2] = param_offset id, ignored
e.transform = Transform(uStr2Float(v[3]), uStr2Float(v[4]), uStr2Float(v[5]),
uStr2Float(v[6]), uStr2Float(v[7]), uStr2Float(v[8]), uStr2Float(v[9]));
e.info = cv::Mat::eye(6, 6, CV_64FC1);
int idx = 10;
for(int r = 0; r < 6; ++r)
{
for(int c = r; c < 6; ++c)
{
e.info.at<double>(r, c) = uStr2Double(v[idx++]);
if(r != c) e.info.at<double>(c, r) = e.info.at<double>(r, c);
}
}
e.type = Link::kPosePrior;
e.isPrior = true;
e.hasLandmarkEndpoint = false;
edgesList.push_back(e);
}
else if(tag == "EDGE_POINTXYZ_PRIOR" && v.size() == 11)
{
EdgeEntry e;
e.from = atoi(v[1].c_str());
e.to = e.from;
e.transform = Transform(uStr2Float(v[2]), uStr2Float(v[3]), uStr2Float(v[4]), 0, 0, 0);
e.info = cv::Mat::eye(6, 6, CV_64FC1);
e.info.at<double>(0, 0) = uStr2Double(v[5]);
e.info.at<double>(0, 1) = e.info.at<double>(1, 0) = uStr2Double(v[6]);
e.info.at<double>(0, 2) = e.info.at<double>(2, 0) = uStr2Double(v[7]);
e.info.at<double>(1, 1) = uStr2Double(v[8]);
e.info.at<double>(1, 2) = e.info.at<double>(2, 1) = uStr2Double(v[9]);
e.info.at<double>(2, 2) = uStr2Double(v[10]);
// no orientation info on this prior
e.info.at<double>(3, 3) = e.info.at<double>(4, 4) = e.info.at<double>(5, 5) = 1.0 / 9999.0;
e.type = Link::kPosePrior;
e.isPrior = true;
e.hasLandmarkEndpoint = false;
edgesList.push_back(e);
}
else if(tag == "EDGE_SE2_SWITCHABLE" && v.size() == 13)
{
EdgeEntry e;
e.from = atoi(v[1].c_str());
e.to = atoi(v[2].c_str());
// v[3] = switch vertex id, ignored
e.transform = Transform(uStr2Float(v[4]), uStr2Float(v[5]), uStr2Float(v[6]));
e.info = cv::Mat::eye(6, 6, CV_64FC1);
e.info.at<double>(0, 0) = uStr2Double(v[7]);
e.info.at<double>(0, 1) = e.info.at<double>(1, 0) = uStr2Double(v[8]);
e.info.at<double>(0, 5) = e.info.at<double>(5, 0) = uStr2Double(v[9]);
e.info.at<double>(1, 1) = uStr2Double(v[10]);
e.info.at<double>(1, 5) = e.info.at<double>(5, 1) = uStr2Double(v[11]);
e.info.at<double>(5, 5) = uStr2Double(v[12]);
e.type = Link::kUndef;
e.isPrior = false;
e.hasLandmarkEndpoint = false;
edgesList.push_back(e);
}
else if(tag == "EDGE_SE3_SWITCHABLE" && v.size() == 32)
{
EdgeEntry e;
e.from = atoi(v[1].c_str());
e.to = atoi(v[2].c_str());
// v[3] = switch vertex id, ignored
e.transform = Transform(uStr2Float(v[4]), uStr2Float(v[5]), uStr2Float(v[6]),
uStr2Float(v[7]), uStr2Float(v[8]), uStr2Float(v[9]), uStr2Float(v[10]));
e.info = cv::Mat::eye(6, 6, CV_64FC1);
int idx = 11;
for(int r = 0; r < 6; ++r)
{
for(int c = r; c < 6; ++c)
{
e.info.at<double>(r, c) = uStr2Double(v[idx++]);
if(r != c) e.info.at<double>(c, r) = e.info.at<double>(r, c);
}
}
e.type = Link::kUndef;
e.isPrior = false;
e.hasLandmarkEndpoint = false;
edgesList.push_back(e);
}
else
{
UWARN("Unsupported or malformed g2o line: \"%s\" (tag=%s, tokens=%d)", line, tag.c_str(), (int)v.size());
}
}
fclose(file);
// Recover landmarkOffset from vertex order:
// file order = [landmarks with DESCENDING file_ids] + [regular poses with ASCENDING file_ids]
// Walk backwards from the end and take the longest ascending suffix as the regular poses.
// landmarkOffset = max regular pose id (= last fileId of that suffix).
int landmarkOffset = 0;
int firstRegularIdx = (int)verticesList.size();
if(!verticesList.empty())
{
firstRegularIdx = (int)verticesList.size() - 1;
while(firstRegularIdx > 0 &&
verticesList[firstRegularIdx - 1].fileId < verticesList[firstRegularIdx].fileId)
{
--firstRegularIdx;
}
landmarkOffset = verticesList.back().fileId;
// If the alleged regular suffix actually starts on a definite landmark
// (VERTEX_XY / VERTEX_TRACKXYZ), then there are no regular poses and
// saveGraph used landmarkOffset = 0; restore that case.
if(verticesList[firstRegularIdx].definitelyLandmark)
{
landmarkOffset = 0;
firstRegularIdx = (int)verticesList.size();
}
}
// Insert vertices into poses, remapping landmark file ids back to negative.
for(int i = 0; i < (int)verticesList.size(); ++i)
{
int originalId;
if(i < firstRegularIdx)
{
originalId = landmarkOffset - verticesList[i].fileId; // negative
}
else
{
originalId = verticesList[i].fileId;
}
if(poses.find(originalId) == poses.end())
{
poses.insert(std::make_pair(originalId, verticesList[i].transform));
}
else
{
UWARN("Vertex %d (file id %d) already exists, ignoring duplicate", originalId, verticesList[i].fileId);
}
}
// Remap edge endpoints. Any file id > landmarkOffset (or, if landmarkOffset == 0
// and there are any landmarks at all, any id present in the landmark prefix)
// is a landmark and gets the negative id back.
bool allLandmarks = (landmarkOffset == 0 && firstRegularIdx == (int)verticesList.size() && !verticesList.empty());
auto remap = [&](int fileId) -> int {
if(landmarkOffset > 0 && fileId > landmarkOffset)
{
return landmarkOffset - fileId; // negative
}
if(allLandmarks)
{
return -fileId;
}
return fileId;
};
for(const EdgeEntry & e : edgesList)
{
int from = remap(e.from);
int to = e.isPrior ? from : remap(e.to);
Link::Type type = e.type;
// Promote ambiguous edges (EDGE_SE2) to kLandmark when an endpoint
// turns out to be a landmark after remapping.
if(type == Link::kUndef && (from < 0 || to < 0))
{
type = Link::kLandmark;
}
edgeConstraints.insert(std::make_pair(from, Link(from, to, type, e.transform, e.info)));
}
UINFO("Graph loaded from %s (%d poses, %d edges, landmarkOffset=%d)",
fileName.c_str(), (int)poses.size(), (int)edgeConstraints.size(), landmarkOffset);
return true;
}
bool OptimizerG2O::saveGraph( bool OptimizerG2O::saveGraph(
const std::string & fileName, const std::string & fileName,
const std::map<int, Transform> & poses, const std::map<int, Transform> & poses,
tools/Export/main.cpp
+9 -8
View File
@@ -1978,11 +1978,12 @@ int main(int argc, char * argv[])
exportPosesFormat, exportPosesFormat,
std::map<int, Transform>(robotPoses.lower_bound(1), robotPoses.end()), std::map<int, Transform>(robotPoses.lower_bound(1), robotPoses.end()),
links, links,
std::map<int, double>(robotStamps.lower_bound(1), robotStamps.end())); std::map<int, double>(robotStamps.lower_bound(1), robotStamps.end()),
parameters);
} }
else else
{ {
rtabmap::graph::exportPoses(outputPath, exportPosesFormat, robotPoses, links, robotStamps); rtabmap::graph::exportPoses(outputPath, exportPosesFormat, robotPoses, links, robotStamps, parameters);
} }
cv::Vec3f vmin, vmax; cv::Vec3f vmin, vmax;
graph::computeMinMax(robotPoses, vmin, vmax); graph::computeMinMax(robotPoses, vmin, vmax);
@@ -2001,7 +2002,7 @@ int main(int argc, char * argv[])
outputPath = outputDirectory+"/"+baseName+"_camera_poses." + posesExt; outputPath = outputDirectory+"/"+baseName+"_camera_poses." + posesExt;
else else
outputPath = outputDirectory+"/"+baseName+"_camera_poses_"+uNumber2Str((int)i)+"." + posesExt; outputPath = outputDirectory+"/"+baseName+"_camera_poses_"+uNumber2Str((int)i)+"." + posesExt;
rtabmap::graph::exportPoses(outputPath, exportPosesFormat, cameraPoses[i], std::multimap<int, Link>(), cameraStamps[i]); rtabmap::graph::exportPoses(outputPath, exportPosesFormat, cameraPoses[i], std::multimap<int, Link>(), cameraStamps[i], parameters);
cv::Vec3f vmin, vmax; cv::Vec3f vmin, vmax;
graph::computeMinMax(cameraPoses[i], vmin, vmax); graph::computeMinMax(cameraPoses[i], vmin, vmax);
printf("%d camera poses exported to \"%s\". (min=[%f,%f,%f] max=[%f,%f,%f])\n", printf("%d camera poses exported to \"%s\". (min=[%f,%f,%f] max=[%f,%f,%f])\n",
@@ -2014,7 +2015,7 @@ int main(int argc, char * argv[])
if(exportPosesScan) if(exportPosesScan)
{ {
std::string outputPath=outputDirectory+"/"+baseName+"_scan_poses." + posesExt; std::string outputPath=outputDirectory+"/"+baseName+"_scan_poses." + posesExt;
rtabmap::graph::exportPoses(outputPath, exportPosesFormat, scanPoses, std::multimap<int, Link>(), scanStamps); rtabmap::graph::exportPoses(outputPath, exportPosesFormat, scanPoses, std::multimap<int, Link>(), scanStamps, parameters);
cv::Vec3f min, max; cv::Vec3f min, max;
graph::computeMinMax(scanPoses, min, max); graph::computeMinMax(scanPoses, min, max);
printf("%d scan poses exported to \"%s\". (min=[%f,%f,%f] max=[%f,%f,%f])\n", printf("%d scan poses exported to \"%s\". (min=[%f,%f,%f] max=[%f,%f,%f])\n",
@@ -2026,7 +2027,7 @@ int main(int argc, char * argv[])
if(exportPosesScan) if(exportPosesScan)
{ {
std::string outputPath=outputDirectory+"/"+baseName+"_scan_poses." + posesExt; std::string outputPath=outputDirectory+"/"+baseName+"_scan_poses." + posesExt;
rtabmap::graph::exportPoses(outputPath, exportPosesFormat, scanPoses, std::multimap<int, Link>(), scanStamps); rtabmap::graph::exportPoses(outputPath, exportPosesFormat, scanPoses, std::multimap<int, Link>(), scanStamps, parameters);
cv::Vec3f min, max; cv::Vec3f min, max;
graph::computeMinMax(scanPoses, min, max); graph::computeMinMax(scanPoses, min, max);
printf("%d scan poses exported to \"%s\". (min=[%f,%f,%f] max=[%f,%f,%f])\n", printf("%d scan poses exported to \"%s\". (min=[%f,%f,%f] max=[%f,%f,%f])\n",
@@ -2038,7 +2039,7 @@ int main(int argc, char * argv[])
if(exportPosesLandmarks) if(exportPosesLandmarks)
{ {
std::string outputPath=outputDirectory+"/"+baseName+"_landmark_poses." + posesExt; std::string outputPath=outputDirectory+"/"+baseName+"_landmark_poses." + posesExt;
rtabmap::graph::exportPoses(outputPath, exportPosesFormat, landmarkPoses, std::multimap<int, Link>(), landmarkStamps); rtabmap::graph::exportPoses(outputPath, exportPosesFormat, landmarkPoses, std::multimap<int, Link>(), landmarkStamps, parameters);
cv::Vec3f min, max; cv::Vec3f min, max;
graph::computeMinMax(landmarkPoses, min, max); graph::computeMinMax(landmarkPoses, min, max);
printf("%d landmark poses exported to \"%s\". (min=[%f,%f,%f] max=[%f,%f,%f])\n", printf("%d landmark poses exported to \"%s\". (min=[%f,%f,%f] max=[%f,%f,%f])\n",
@@ -2050,7 +2051,7 @@ int main(int argc, char * argv[])
if(exportPosesGps) if(exportPosesGps)
{ {
std::string outputPath=outputDirectory+"/"+baseName+"_gps_poses." + posesExt; std::string outputPath=outputDirectory+"/"+baseName+"_gps_poses." + posesExt;
rtabmap::graph::exportPoses(outputPath, exportPosesFormat, gpsPoses, std::multimap<int, Link>(), gpsStamps); rtabmap::graph::exportPoses(outputPath, exportPosesFormat, gpsPoses, std::multimap<int, Link>(), gpsStamps, parameters);
printf("%d GPS poses exported to \"%s\".\n", printf("%d GPS poses exported to \"%s\".\n",
(int)gpsPoses.size(), (int)gpsPoses.size(),
outputPath.c_str()); outputPath.c_str());
@@ -2058,7 +2059,7 @@ int main(int argc, char * argv[])
if(exportPosesGt) if(exportPosesGt)
{ {
std::string outputPath=outputDirectory+"/"+baseName+"_gt_poses." + posesExt; std::string outputPath=outputDirectory+"/"+baseName+"_gt_poses." + posesExt;
rtabmap::graph::exportPoses(outputPath, exportPosesFormat, gtPoses, std::multimap<int, Link>(), gtStamps); rtabmap::graph::exportPoses(outputPath, exportPosesFormat, gtPoses, std::multimap<int, Link>(), gtStamps, parameters);
printf("%d scan poses exported to \"%s\".\n", printf("%d scan poses exported to \"%s\".\n",
(int)gtPoses.size(), (int)gtPoses.size(),
outputPath.c_str()); outputPath.c_str());