OptmizerG2O: add prior edge export #319
Conversation
|
Can you test this with g2o tools? I am not sure that EDGE_SE3_PRIOR:QUAT requires 2 ids, instead of: It could be something like this (while avoiding problems when robust constraints are exported): std::string prefix = "EDGE_SE3:QUAT";
std::string suffix = "";
std::string to = uFormat(" %d", iter->second.to());
if(useRobustConstraints &&
iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged &&
iter->second.type() != Link::kPosePrior)
{
prefix = "EDGE_SE3_SWITCHABLE";
fprintf(file, "VERTEX_SWITCH %d 1\n", virtualVertexId);
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:QUAT";
to = "";
}
Eigen::Quaternionf q = iter->second.transform().getQuaternionf();
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(),
to.c_str(),
suffix.c_str(),
... |
|
Sure will test and let you know. Thanks |
|
@matlabbe I just tried with both my code and also with the suggestion above on Note: |
|
Update: having the prior edges being printed as or even I get on |
TSC21
force-pushed
the
pr-add_prior_edge_exports
branch
from
3fa28ce to
5db9550
Compare
TSC21
force-pushed
the
pr-add_prior_edge_exports
branch
from
5db9550 to
d6d7463
Compare
|
@matlabbe taking a look at #188 (comment), seems to me that it has two ID's. |
|
The read function for EDGE_SE3_PRIOR is here: There is a pid parameter. In rtabmap the priors are used only if The pid is set here:PARAM_OFFSET is set here:So if it requires the edge id and a Also if you don't want priors to be exported when else if(iter->second.type() == Link::kPosePrior)
{
if(priorsIgnored())
{
continue;
}
prefix = "EDGE_SE3_PRIOR";
to = uFormat(" %d", PARAM_OFFSET);
}With Another problem with your example above, you have commas instead of dots for the float parameters. This is a problem when reading back the file (on my computer the language assumes dots for floating points). I replaced all commas, then I tested the following file and it works in |
|
@matlabbe considering the following line: https://github.com/introlab/rtabmap/pull/319/files#diff-2f635a7c60d30c05905f641c8a9637c8R1372, I continue to not be able to have the ROS launch parameter to be accepted on the rtabmap side. |
TSC21
force-pushed
the
pr-add_prior_edge_exports
branch
from
7969bae to
5da610f
Compare
|
I think I didn't follow all the different emails. priorsIgnored() should work in optimize() function, but the saveGraph() function is static, which means checking priorsIgnored() is meaningless. It is why there is a I realize that even robust parameter is not used, a deep refactoring is needed. I think for now set yours I opened an issue here for the deep refactoring needed: #320 cheers, |
@matlabbe I am aware of that and that's why I created a |
|
The Parameters class is not a singleton, the "Parameters::default*******()" only gets the default value of the parameter, which will be always true here for priors ignored. The more general way would be to make saveGraph() not static, then use it like the following. Instead of Line 77 in 527cbdd this would be: OptimizerG2O g2o(parameters);
return g2o.saveGraph(tmpPath, poses, constraints);
|
OptimizerG2O g2o(parameters);
return g2o.saveGraph(tmpPath, poses, constraints);The question is: where do I get these |
|
If I actually try to bring the parameter passing through |
|
@matlabbe can you review and if ok bring this in the current state? It currently does export the priors as supposed, which was the first intent of this PR. We can work on a way of making the exportation of the priors and robust vertex in a further iteration. Thanks! |
TSC21
force-pushed
the
pr-add_prior_edge_exports
branch
from
1c86f41 to
6126503
Compare
|
The windows compiler is not finding setLocale() in std. Based on this example (I cannot test myself right now on Windows), can you try adding |
|
@matlabbe done! |
|
Thanks for merging! |
Example output: