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

8
corelib/src/Graph.cpp
View File

@@ -56,7 +56,7 @@ bool exportPoses(
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & constraints, // required for formats 3 and 4
const std::map<int, double> & stamps, // required for format 1
bool g2oRobust) // optional for format 4
const bool g2oRobust) // optional for format 4
{
UDEBUG("%s", filePath.c_str());
std::string tmpPath = filePath;
@@ -366,8 +366,8 @@ bool importPoses(
std::vector<std::string> strList = uListToVector(uSplit(str));
if(strList.size() == 3)
{
if( uIsNumber(uReplaceChar(strList[0], ' ', "")) &&
uIsNumber(uReplaceChar(strList[1], ' ', "")) &&
if( uIsNumber(uReplaceChar(strList[0], ' ', "")) &&
uIsNumber(uReplaceChar(strList[1], ' ', "")) &&
uIsNumber(uReplaceChar(strList[2], ' ', "")) &&
(strList.size()==3 || uIsNumber(uReplaceChar(strList[3], ' ', ""))))
{
@@ -379,7 +379,7 @@ bool importPoses(
{
stamps->insert(std::make_pair(id, stamp));
}
float yaw = 0.0f;
float yaw = 0.0f;
if(uContains(poses, id-1))
{
// set yaw depending on successive poses

4
corelib/src/Rtabmap.cpp
View File

@@ -2304,7 +2304,7 @@ bool Rtabmap::process(
UINFO("Update map correction");
std::map<int, Transform> poses = _optimizedPoses;
// if _optimizeFromGraphEnd parameter just changed state, don't use optimized poses as guess
float normMapCorrection = _mapCorrection.getNormSquared(); // use distance for identity detection
if((normMapCorrection > 0.000001f && _optimizeFromGraphEnd) ||
@@ -4694,7 +4694,7 @@ void Rtabmap::updateGoalIndex()
this->clearPath(-1);
return;
}
}
}
}
else if(!isStuck)
{

4
corelib/src/RtabmapThread.cpp
View File

@@ -348,12 +348,12 @@ bool RtabmapThread::handleEvent(UEvent* event)
}
}
else
{
{
OdometryInfo infoCov;
infoCov.reg.covariance = e->info().odomCovariance;
this->addData(OdometryEvent(e->data(), e->info().odomPose, infoCov));
}
}
}
else if(event->getClassName().compare("OdometryEvent") == 0)

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