i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-08 04:20:20 +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 11:58:57 -04: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
corelib/include/rtabmap/core/optimizer/OptimizerG2O.h
+2 -2
View File
@@ -44,14 +44,14 @@ public:
const std::string & fileName,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints,
bool useRobustConstraints = false);
const bool useRobustConstraints = false);
public:
OptimizerG2O(const ParametersMap & parameters = ParametersMap()) :
Optimizer(parameters),
solver_(Parameters::defaultg2oSolver()),
optimizer_(Parameters::defaultg2oOptimizer()),
pixelVariance_(Parameters::defaultg2oPixelVariance()),
pixelVariance_(Parameters::defaultg2oPixelVariance()),
robustKernelDelta_(Parameters::defaultg2oRobustKernelDelta()),
baseline_(Parameters::defaultg2oBaseline())
{
corelib/src/Graph.cpp
+4 -4
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
corelib/src/Rtabmap.cpp
+2 -2
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)
{
corelib/src/RtabmapThread.cpp
+2 -2
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)
corelib/src/optimizer/OptimizerG2O.cpp
+29 -4
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
guilib/src/MainWindow.cpp
+10 -10
View File
@@ -208,9 +208,9 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
// Add cloud viewers
// Note that we add them here manually because there is a crash issue
// when adding them in a DockWidget of the *.ui file. The cloud viewer is
// when adding them in a DockWidget of the *.ui file. The cloud viewer is
// created in a widget which is not yet linked to main window when the CloudViewer constructor
// is called (see order in generated ui file). VTK needs to get the top
// is called (see order in generated ui file). VTK needs to get the top
// level window at the time CloudViewer is created, otherwise it may crash on some systems.
_cloudViewer = new CloudViewer(_ui->layout_cloudViewer);
_cloudViewer->setObjectName("widget_cloudViewer");
@@ -394,7 +394,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
_ui->actionPause->setShortcut(Qt::Key_Space);
_ui->actionSave_GUI_config->setShortcut(QKeySequence::Save);
// Qt5 issue, we should explicitly add actions not in
// Qt5 issue, we should explicitly add actions not in
// menu bar to have shortcut working
this->addAction(_ui->actionSave_GUI_config);
_ui->actionReset_Odometry->setEnabled(false);
@@ -670,23 +670,23 @@ void MainWindow::setupMainLayout(bool vertical)
}
void MainWindow::setCloudViewer(rtabmap::CloudViewer * cloudViewer)
{
UASSERT(cloudViewer);
{
UASSERT(cloudViewer);
delete _cloudViewer;
_cloudViewer = cloudViewer;
_cloudViewer = cloudViewer;
_cloudViewer->setParent(_ui->layout_cloudViewer);
_cloudViewer->setObjectName("widget_cloudViewer");
_ui->layout_cloudViewer->layout()->addWidget(_cloudViewer);
_cloudViewer->setBackfaceCulling(true, false);
_preferencesDialog->loadWidgetState(_cloudViewer);
connect(_cloudViewer, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
}
void MainWindow::setLoopClosureViewer(rtabmap::LoopClosureViewer * loopClosureViewer)
{
UASSERT(loopClosureViewer);
delete _loopClosureViewer;
{
UASSERT(loopClosureViewer);
delete _loopClosureViewer;
_loopClosureViewer = loopClosureViewer;
_loopClosureViewer->setParent(_ui->layout_loopClosureViewer);
_loopClosureViewer->setObjectName("widget_loopClosureViewer");