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

OptmizerG2O: add prior edge export (#319)

Browse Source 
* OptmizerG2O: add prior edge export

* add required conditions and structure to export prior edges

* for now, make the graph exportation of priors not dependent of parameters

* include <locale.h> so to use setlocale() in a windows build
This commit is contained in:
Nuno Marques
2018-10-15 16:58:57 +01:00
committed by matlabbe
parent 444b511548
commit 42c60c7154
6 changed files with 49 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
corelib/src/optimizer/OptimizerG2O.cpp
View File

@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UTimer.h>
#include <locale.h>
#include <set>
#include <rtabmap/core/Version.h>
@@ -1363,7 +1364,7 @@ bool OptimizerG2O::saveGraph(
const std::string & fileName,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints,
bool useRobustConstraints)
const bool useRobustConstraints)
{
FILE * file = 0;
@@ -1375,6 +1376,22 @@ bool OptimizerG2O::saveGraph(
if(file)
{
// force periods to be used instead of commas
setlocale(LC_ALL, "en_US.UTF-8");
// PARAMS_SE3OFFSET id x y z qw qx qy qz (set for priors)
Eigen::Vector3f v = Eigen::Vector3f::Zero();
Eigen::Quaternionf q = Eigen::Quaternionf::Identity();
fprintf(file, "PARAMS_SE3OFFSET %d %f %f %f %f %f %f %f\n",
PARAM_OFFSET,
v.x(),
v.y(),
v.z(),
q.x(),
q.y(),
q.z(),
q.w());
// VERTEX_SE3 id x y z qw qx qy qz
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
@@ -1390,12 +1407,14 @@ bool OptimizerG2O::saveGraph(
q.w());
}
//EDGE_SE3 observed_vertex_id observing_vertex_id x y z qx qy qz qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
// EDGE_SE3 observed_vertex_id observing_vertex_id x y z qx qy qz qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
// EDGE_SE3_PRIOR observed_vertex_id x y z qx qy qz qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
int virtualVertexId = poses.size()?poses.rbegin()->first+1:0;
for(std::multimap<int, Link>::const_iterator iter = edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
{
std::string prefix = "EDGE_SE3:QUAT";
std::string suffix = "";
std::string to = uFormat(" %d", iter->second.to());
if(useRobustConstraints &&
iter->second.type() != Link::kNeighbor &&
@@ -1406,12 +1425,17 @@ bool OptimizerG2O::saveGraph(
fprintf(file, "EDGE_SWITCH_PRIOR %d 1 1.0\n", virtualVertexId);
suffix = uFormat(" %d", virtualVertexId++);
}
else if(iter->second.type() == Link::kPosePrior)
{
prefix = "EDGE_SE3_PRIOR";
to = uFormat(" %d", PARAM_OFFSET);
}
Eigen::Quaternionf q = iter->second.transform().getQuaternionf();
fprintf(file, "%s %d %d%s %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
fprintf(file, "%s %d%s%s %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
prefix.c_str(),
iter->second.from(),
iter->second.to(),
to.c_str(),
suffix.c_str(),
iter->second.transform().x(),
iter->second.transform().y(),
@@ -1443,6 +1467,7 @@ bool OptimizerG2O::saveGraph(
iter->second.infMatrix().at<double>(5,5));
}
UINFO("Graph saved to %s", fileName.c_str());
fclose(file);
}
else