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

Added g2o optimization option (along with TORO). Some refactoring: updated graph optimization parameter names, new graph::Optimizer class and new parameter RGBD/OptimizeSlam2d

Browse Source
This commit is contained in:
Mathieu Labbe
2015-03-12 17:00:56 -04:00
parent 05d4276ba0
commit aa8fe2e55c
22 changed files with 3254 additions and 820 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+8 -1
View File
@@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
#######################
SET(RTABMAP_MAJOR_VERSION 0)
SET(RTABMAP_MINOR_VERSION 8)
SET(RTABMAP_PATCH_VERSION 6)
SET(RTABMAP_PATCH_VERSION 7)
SET(RTABMAP_VERSION
${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
@@ -125,6 +125,7 @@ ENDIF("${VTK_MAJOR_VERSION}" EQUAL 5)
FIND_PACKAGE(ZLIB REQUIRED)
FIND_PACKAGE(Freenect)
FIND_PACKAGE(OpenNI2)
FIND_PACKAGE(G2O)
# If Qt is here, the GUI will be built
FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg)
@@ -328,4 +329,10 @@ ELSE()
MESSAGE(STATUS " With OpenNI2 = NO (OpenNI2 not found)")
ENDIF()
IF(G2O_FOUND)
MESSAGE(STATUS " With g2o = YES")
ELSE()
MESSAGE(STATUS " With g2o = NO (g2o not found)")
ENDIF()
MESSAGE(STATUS "--------------------------------------------")
cmake_modules/FindG2O.cmake
+80
View File
@@ -0,0 +1,80 @@
#Pre-requisites: Look for csparse
FIND_PATH(CSPARSE_INCLUDE_DIR NAMES cs.h PATH_SUFFIXES suitesparse )
FIND_LIBRARY(CSPARSE_LIBRARY NAMES cxsparse)
include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(CSPARSE DEFAULT_MSG CSPARSE_INCLUDE_DIR CSPARSE_LIBRARY)
# G2O: Find the header files
FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h)
# Macro to unify finding both the debug and release versions of the
# libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY
# macro.
MACRO(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME)
FIND_LIBRARY("${MYLIBRARY}_DEBUG"
NAMES "g2o_${MYLIBRARYNAME}_d"
)
FIND_LIBRARY(${MYLIBRARY}
NAMES "g2o_${MYLIBRARYNAME}"
)
IF(NOT ${MYLIBRARY}_DEBUG)
IF(MYLIBRARY)
SET(${MYLIBRARY}_DEBUG ${MYLIBRARY})
ENDIF(MYLIBRARY)
ENDIF( NOT ${MYLIBRARY}_DEBUG)
ENDMACRO(FIND_G2O_LIBRARY LIBRARY LIBRARYNAME)
# Find the core elements
FIND_G2O_LIBRARY(G2O_STUFF_LIBRARY stuff)
FIND_G2O_LIBRARY(G2O_CORE_LIBRARY core)
# Find the CLI library
FIND_G2O_LIBRARY(G2O_CLI_LIBRARY cli)
# Find the pluggable solvers
FIND_G2O_LIBRARY(G2O_SOLVER_CHOLMOD solver_cholmod)
FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE solver_csparse)
FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE_EXTENSION csparse_extension)
FIND_G2O_LIBRARY(G2O_SOLVER_DENSE solver_dense)
FIND_G2O_LIBRARY(G2O_SOLVER_PCG solver_pcg)
FIND_G2O_LIBRARY(G2O_SOLVER_SLAM2D_LINEAR solver_slam2d_linear)
FIND_G2O_LIBRARY(G2O_SOLVER_STRUCTURE_ONLY solver_structure_only)
FIND_G2O_LIBRARY(G2O_SOLVER_EIGEN solver_eigen)
# Find the predefined types
FIND_G2O_LIBRARY(G2O_TYPES_DATA types_data)
FIND_G2O_LIBRARY(G2O_TYPES_ICP types_icp)
FIND_G2O_LIBRARY(G2O_TYPES_SBA types_sba)
FIND_G2O_LIBRARY(G2O_TYPES_SCLAM2D types_sclam2d)
FIND_G2O_LIBRARY(G2O_TYPES_SIM3 types_sim3)
FIND_G2O_LIBRARY(G2O_TYPES_SLAM2D types_slam2d)
FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D types_slam3d)
# G2O solvers declared found if we found at least one solver
SET(G2O_SOLVERS_FOUND "NO")
IF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN)
SET(G2O_SOLVERS_FOUND "YES")
ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN)
# G2O itself declared found if we found the core libraries and at least one solver
SET(G2O_FOUND "NO")
IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND AND CSPARSE_FOUND)
SET(G2O_INCLUDE_DIRS ${G2O_INCLUDE_DIR} ${CSPARSE_INCLUDE_DIR})
SET(G2O_LIBRARIES
${G2O_STUFF_LIBRARY}
${G2O_CORE_LIBRARY}
${G2O_SOLVER_CSPARSE}
${G2O_SOLVER_CSPARSE_EXTENSION}
${G2O_TYPES_SLAM2D}
${G2O_TYPES_SLAM3D}
${CSPARSE_LIBRARY})
SET(G2O_FOUND "YES")
ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND AND CSPARSE_FOUND)
corelib/include/rtabmap/core/Graph.h
+103 -34
View File
@@ -33,50 +33,119 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <map>
#include <list>
#include <rtabmap/core/Link.h>
#include <rtabmap/core/Parameters.h>
namespace rtabmap {
namespace graph {
////////////////////////////////////////////
// Graph optimizers
////////////////////////////////////////////
class RTABMAP_EXP Optimizer
{
public:
enum Type {
kTypeUndef = -1,
kTypeTORO = 0,
kTypeG2O = 1
};
static Optimizer * create(const ParametersMap & parameters);
static Optimizer * create(Optimizer::Type & type, const ParametersMap & parameters = ParametersMap());
// Get connected poses and constraints from a set of links
static void getConnectedGraph(
int fromId,
const std::map<int, Transform> & posesIn,
const std::multimap<int, Link> & linksIn,
std::map<int, Transform> & posesOut,
std::multimap<int, Link> & linksOut,
int depth = 0);
public:
virtual ~Optimizer() {}
virtual Type type() const = 0;
int iterations() const {return iterations_;}
bool isSlam2d() const {return slam2d_;}
bool isCovarianceIgnored() const {return covarianceIgnored_;}
virtual std::map<int, Transform> optimize(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & constraints,
std::list<std::map<int, Transform> > * intermediateGraphes = 0) = 0;
virtual void parseParameters(const ParametersMap & parameters);
protected:
Optimizer(int iterations = 100, bool slam2d = false, bool covarianceIgnored = false);
Optimizer(const ParametersMap & parameters);
private:
int iterations_;
bool slam2d_;
bool covarianceIgnored_;
};
class RTABMAP_EXP TOROOptimizer : public Optimizer
{
public:
static bool saveGraph(
const std::string & fileName,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints);
static bool loadGraph(
const std::string & fileName,
std::map<int, Transform> & poses,
std::multimap<int, std::pair<int, Transform> > & edgeConstraints);
public:
TOROOptimizer(int iterations = 100, bool slam2d = false, bool covarianceIgnored = false) :
Optimizer(iterations, slam2d, covarianceIgnored) {}
TOROOptimizer(const ParametersMap & parameters) :
Optimizer(parameters) {}
virtual ~TOROOptimizer() {}
virtual Type type() const {return kTypeTORO;}
virtual std::map<int, Transform> optimize(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints,
std::list<std::map<int, Transform> > * intermediateGraphes = 0);
};
class RTABMAP_EXP G2OOptimizer : public Optimizer
{
public:
static bool available();
public:
G2OOptimizer(int iterations = 100, bool slam2d = false, bool covarianceIgnored = false) :
Optimizer(iterations, slam2d, covarianceIgnored) {}
G2OOptimizer(const ParametersMap & parameters) :
Optimizer(parameters) {}
virtual ~G2OOptimizer() {}
virtual Type type() const {return kTypeG2O;}
virtual std::map<int, Transform> optimize(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints,
std::list<std::map<int, Transform> > * intermediateGraphes = 0);
};
////////////////////////////////////////////
// Graph utilities
////////////////////////////////////////////
std::multimap<int, Link>::iterator RTABMAP_EXP findLink(
std::multimap<int, Link> & links,
int from,
int to);
// <int, depth> depth=0 means infinite depth
std::map<int, int> RTABMAP_EXP generateDepthGraph(
const std::multimap<int, Link> & links,
int fromId,
int depth = 0);
void RTABMAP_EXP optimizeTOROGraph(
const std::map<int, int> & depthGraph,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
std::map<int, Transform> & optimizedPoses,
int toroIterations = 100,
bool toroInitialGuess = true,
bool ignoreCovariance = false,
std::list<std::map<int, Transform> > * intermediateGraphes = 0);
void RTABMAP_EXP optimizeTOROGraph(
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints,
std::map<int, Transform> & optimizedPoses,
int toroIterations = 100,
bool toroInitialGuess = true,
bool ignoreCovariance = false,
std::list<std::map<int, Transform> > * intermediateGraphes = 0);
bool RTABMAP_EXP saveTOROGraph(
const std::string & fileName,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints);
bool RTABMAP_EXP loadTOROGraph(const std::string & fileName,
std::map<int, Transform> & poses,
std::multimap<int, std::pair<int, Transform> > & edgeConstraints);
/**
* Get only the the most recent or older poses in the defined radius.
* @param poses The poses
corelib/include/rtabmap/core/Parameters.h
+6 -2
View File
@@ -287,8 +287,6 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(RGBD, LinearUpdate, float, 0.0, "Min linear displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
RTABMAP_PARAM(RGBD, AngularUpdate, float, 0.0, "Min angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0, "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
RTABMAP_PARAM(RGBD, ToroIterations, int, 100, "TORO graph optimization iterations");
RTABMAP_PARAM(RGBD, ToroIgnoreVariance, bool, false, "Ignore constraints' variance. If checked, identity information matrix is used for each constraint in TORO. Otherwise, an information matrix is generated from the variance saved in the links.");
RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd, bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
RTABMAP_PARAM(RGBD, GoalReachedRadius, float, 0.5, "Goal reached radius (m).");
RTABMAP_PARAM(RGBD, PlanWithNearNodesLinked, bool, true, "Before planning in the graph, near nodes are linked together (even if they don't belong to same map). Radius is defined by \"RGBD/GoalReachedRadius\" parameter.");
@@ -301,6 +299,12 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(RGBD, LocalLoopDetectionNeighbors, int, 20, "Maximum nearest neighbor.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionMaxDiffID, int, 50, "Maximum ID difference between the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.")
// Graph optimization
RTABMAP_PARAM(RGBD, OptimizeStrategy, int, 0, "Graph optimization strategy: 0=TORO and 1=g2o.");
RTABMAP_PARAM(RGBD, OptimizeIterations, int, 100, "Optimization iterations.");
RTABMAP_PARAM(RGBD, OptimizeSlam2D, bool, false, "If optimization is done only on x,y and theta (3DoF). Otherwise, it is done on full 6DoF poses.");
RTABMAP_PARAM(RGBD, OptimizeVarianceIgnored, bool, false, "Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links.");
// Odometry
RTABMAP_PARAM(Odom, Strategy, int, 0, "0=Bag-of-words 1=Optical Flow");
RTABMAP_PARAM(Odom, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
corelib/include/rtabmap/core/Rtabmap.h
+4 -2
View File
@@ -47,6 +47,9 @@ class EpipolarGeometry;
class Memory;
class BayesFilter;
class Signature;
namespace graph {
class Optimizer;
}
class RTABMAP_EXP Rtabmap
{
@@ -174,8 +177,6 @@ private:
float _localRadius;
float _localDetectMaxNeighbors;
int _localDetectMaxDiffID;
int _toroIterations;
bool _toroIgnoreVariance;
std::string _databasePath;
bool _optimizeFromGraphEnd;
bool _reextractLoopClosureFeatures;
@@ -195,6 +196,7 @@ private:
// strategies for a type of signature or configuration.
EpipolarGeometry * _epipolarGeometry;
BayesFilter * _bayesFilter;
graph::Optimizer * _graphOptimizer;
ParametersMap _lastParameters;
Memory * _memory;
corelib/include/rtabmap/core/Transform.h
+8
View File
@@ -83,11 +83,14 @@ public:
const float & y() const {return data_[7];}
const float & z() const {return data_[11];}
float theta() const;
Transform inverse() const;
Transform rotation() const;
Transform translation() const;
void getTranslationAndEulerAngles(float & x, float & y, float & z, float & roll, float & pitch, float & yaw) const;
void getEulerAngles(float & roll, float & pitch, float & yaw) const;
void getTranslation(float & x, float & y, float & z) const;
float getNorm() const;
float getNormSquared() const;
@@ -105,12 +108,17 @@ public:
Eigen::Affine3f toEigen3f() const;
Eigen::Affine3d toEigen3d() const;
Eigen::Quaternionf getQuaternionf() const;
Eigen::Quaterniond getQuaterniond() const;
public:
static Transform getIdentity();
static Transform fromEigen4f(const Eigen::Matrix4f & matrix);
static Transform fromEigen4d(const Eigen::Matrix4d & matrix);
static Transform fromEigen3f(const Eigen::Affine3f & matrix);
static Transform fromEigen3d(const Eigen::Affine3d & matrix);
static Transform fromEigen3f(const Eigen::Isometry3f & matrix);
static Transform fromEigen3d(const Eigen::Isometry3d & matrix);
private:
std::vector<float> data_;
corelib/src/CMakeLists.txt
+15
View File
@@ -34,6 +34,9 @@ SET(SRC_FILES
toro3d/treeoptimizer3_iteration.cpp
toro3d/treeoptimizer3.cpp
toro3d/posegraph2.cpp
toro3d/treeoptimizer2.cpp
sqlite3/sqlite3.c
)
@@ -80,6 +83,18 @@ IF(OpenNI2_FOUND)
)
ENDIF(OpenNI2_FOUND)
IF(G2O_FOUND)
ADD_DEFINITIONS("-DWITH_G2O")
SET(INCLUDE_DIRS
${INCLUDE_DIRS}
${G2O_INCLUDE_DIRS}
)
SET(LIBRARIES
${LIBRARIES}
${G2O_LIBRARIES}
)
ENDIF(G2O_FOUND)
####################################
# Generate resources files
####################################
corelib/src/Graph.cpp
+590 -256
View File
@@ -36,54 +36,118 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <set>
#include <queue>
#include "toro3d/treeoptimizer3.hh"
#include "toro3d/treeoptimizer2.hh"
#ifdef WITH_G2O
#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/factory.h"
#include "g2o/core/optimization_algorithm_factory.h"
#include "g2o/core/optimization_algorithm_gauss_newton.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/solvers/csparse/linear_solver_csparse.h"
#include "g2o/types/slam3d/vertex_se3.h"
#include "g2o/types/slam3d/edge_se3.h"
#include "g2o/types/slam2d/vertex_se2.h"
#include "g2o/types/slam2d/edge_se2.h"
#endif
namespace rtabmap {
namespace graph {
std::multimap<int, Link>::iterator findLink(
std::multimap<int, Link> & links,
int from,
int to)
{
std::multimap<int, Link>::iterator iter = links.find(from);
while(iter != links.end() && iter->first == from)
{
if(iter->second.to() == to)
{
return iter;
}
++iter;
}
////////////////////////////////////////////
// Graph optimizers
////////////////////////////////////////////
// let's try to -> from
iter = links.find(to);
while(iter != links.end() && iter->first == to)
Optimizer * Optimizer::create(const ParametersMap & parameters)
{
int optimizerTypeInt = Parameters::defaultRGBDOptimizeStrategy();
Parameters::parse(parameters, Parameters::kRGBDOptimizeStrategy(), optimizerTypeInt);
graph::Optimizer::Type type = (graph::Optimizer::Type)optimizerTypeInt;
if(!G2OOptimizer::available() && type == Optimizer::kTypeG2O)
{
if(iter->second.to() == from)
{
return iter;
}
++iter;
UWARN("g2o optimizer not available. TORO will be used instead.");
type = Optimizer::kTypeTORO;
}
return links.end();
Optimizer * optimizer = 0;
switch(type)
{
case Optimizer::kTypeG2O:
optimizer = new G2OOptimizer(parameters);
break;
case Optimizer::kTypeTORO:
default:
optimizer = new TOROOptimizer(parameters);
type = Optimizer::kTypeTORO;
break;
}
return optimizer;
}
Optimizer * Optimizer::create(Optimizer::Type & type, const ParametersMap & parameters)
{
if(!G2OOptimizer::available() && type == Optimizer::kTypeG2O)
{
UWARN("g2o optimizer not available. TORO will be used instead.");
type = Optimizer::kTypeTORO;
}
Optimizer * optimizer = 0;
switch(type)
{
case Optimizer::kTypeG2O:
optimizer = new G2OOptimizer(parameters);
break;
case Optimizer::kTypeTORO:
default:
optimizer = new TOROOptimizer(parameters);
type = Optimizer::kTypeTORO;
break;
// <int, depth> margin=0 means infinite margin
std::map<int, int> generateDepthGraph(
const std::multimap<int, Link> & links,
}
return optimizer;
}
Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored) :
iterations_(iterations),
slam2d_(slam2d),
covarianceIgnored_(covarianceIgnored)
{
}
Optimizer::Optimizer(const ParametersMap & parameters) :
iterations_(100),
slam2d_(false),
covarianceIgnored_(false)
{
parseParameters(parameters);
}
void Optimizer::parseParameters(const ParametersMap & parameters)
{
Parameters::parse(parameters, Parameters::kRGBDOptimizeIterations(), iterations_);
Parameters::parse(parameters, Parameters::kRGBDOptimizeVarianceIgnored(), covarianceIgnored_);
Parameters::parse(parameters, Parameters::kRGBDOptimizeSlam2D(), slam2d_);
}
void Optimizer::getConnectedGraph(
int fromId,
const std::map<int, Transform> & posesIn,
const std::multimap<int, Link> & linksIn,
std::map<int, Transform> & posesOut,
std::multimap<int, Link> & linksOut,
int depth)
{
UASSERT(depth >= 0);
//UDEBUG("signatureId=%d, neighborsMargin=%d", signatureId, margin);
std::map<int, int> ids;
if(fromId<=0)
{
return ids;
}
UASSERT(fromId>0);
UASSERT(uContains(posesIn, fromId));
posesOut.clear();
linksOut.clear();
std::set<int> ids;
std::list<int> curentDepthList;
std::set<int> nextDepth;
nextDepth.insert(fromId);
@@ -97,288 +161,250 @@ std::map<int, int> generateDepthGraph(
{
if(ids.find(*jter) == ids.end())
{
std::set<int> marginIds;
ids.insert(*jter);
posesOut.insert(*posesIn.find(*jter));
ids.insert(std::pair<int, int>(*jter, d));
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
for(std::multimap<int, Link>::const_iterator iter=linksIn.begin(); iter!=linksIn.end(); ++iter)
{
if(iter->second.from() == *jter)
{
marginIds.insert(iter->second.to());
if(ids.find(iter->second.to()) == ids.end() && uContains(posesIn, iter->second.to()))
{
linksOut.insert(*iter);
nextDepth.insert(iter->second.to());
}
}
else if(iter->second.to() == *jter)
{
marginIds.insert(iter->second.from());
}
}
// Margin links
for(std::set<int>::const_iterator iter=marginIds.begin(); iter!=marginIds.end(); ++iter)
{
if( !uContains(ids, *iter) && nextDepth.find(*iter) == nextDepth.end())
{
nextDepth.insert(*iter);
if(ids.find(iter->second.from()) == ids.end() && uContains(posesIn, iter->second.from()))
{
linksOut.insert(*iter);
nextDepth.insert(iter->second.from());
}
}
}
}
}
++d;
}
return ids;
}
void optimizeTOROGraph(
const std::map<int, int> & depthGraph,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
std::map<int, Transform> & optimizedPoses,
int toroIterations,
bool toroInitialGuess,
bool ignoreCovariance,
std::list<std::map<int, Transform> > * intermediateGraphes)
{
optimizedPoses.clear();
if(depthGraph.size() >= 2 && poses.size()>=2 && links.size()>=1)
{
// Modify IDs using the margin from the current signature (TORO root will be the last signature)
int m = 0;
int toroId = 1;
std::map<int, int> rtabmapToToro; // <RTAB-Map ID, TORO ID>
std::map<int, int> toroToRtabmap; // <TORO ID, RTAB-Map ID>
std::map<int, int> idsTmp = depthGraph;
while(idsTmp.size())
{
for(std::map<int, int>::iterator iter = idsTmp.begin(); iter!=idsTmp.end();)
{
if(m == iter->second)
{
rtabmapToToro.insert(std::make_pair(iter->first, toroId));
toroToRtabmap.insert(std::make_pair(toroId, iter->first));
++toroId;
idsTmp.erase(iter++);
}
else
{
++iter;
}
}
++m;
}
std::map<int, rtabmap::Transform> posesToro;
std::multimap<int, rtabmap::Link> edgeConstraintsToro;
for(std::map<int, rtabmap::Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
if(uContains(depthGraph, iter->first))
{
UASSERT(uContains(rtabmapToToro, iter->first));
UASSERT_MSG(!iter->second.isNull(), uFormat("Poses should not be null! Id=%d", iter->first).c_str());
posesToro.insert(std::make_pair(rtabmapToToro.at(iter->first), iter->second));
}
}
for(std::multimap<int, rtabmap::Link>::const_iterator iter = links.begin();
iter!=links.end();
++iter)
{
if(uContains(depthGraph, iter->second.from()) && uContains(depthGraph, iter->second.to()))
{
UASSERT(uContains(rtabmapToToro, iter->first) && uContains(rtabmapToToro, iter->second.to()));
UASSERT_MSG(!iter->second.transform().isNull(), uFormat("Link from=%d to=%d", iter->first, iter->second.to()).c_str());
edgeConstraintsToro.insert(std::make_pair(rtabmapToToro.at(iter->first), Link(rtabmapToToro.at(iter->first), rtabmapToToro.at(iter->second.to()), iter->second.type(), iter->second.transform(), iter->second.rotVariance(), iter->second.transVariance())));
}
}
std::map<int, rtabmap::Transform> optimizedPosesToro;
if(posesToro.size() && edgeConstraintsToro.size())
{
std::list<std::map<int, rtabmap::Transform> > graphesToro;
// Optimize!
optimizeTOROGraph(
posesToro,
edgeConstraintsToro,
optimizedPosesToro,
toroIterations,
toroInitialGuess,
ignoreCovariance,
&graphesToro);
for(std::map<int, rtabmap::Transform>::iterator iter=optimizedPosesToro.begin(); iter!=optimizedPosesToro.end(); ++iter)
{
optimizedPoses.insert(std::make_pair(toroToRtabmap.at(iter->first), iter->second));
}
if(intermediateGraphes)
{
for(std::list<std::map<int, rtabmap::Transform> >::iterator iter = graphesToro.begin(); iter!=graphesToro.end(); ++iter)
{
std::map<int, rtabmap::Transform> tmp;
for(std::map<int, rtabmap::Transform>::iterator jter=iter->begin(); jter!=iter->end(); ++jter)
{
tmp.insert(std::make_pair(toroToRtabmap.at(jter->first), jter->second));
}
intermediateGraphes->push_back(tmp);
}
}
}
else
{
if(edgeConstraintsToro.size() == 0)
{
UERROR("No TORO constraints!? (input poses=%d, links=%d, depthGraph=%d)",
(int)poses.size(), (int)links.size(), (int)depthGraph.size());
}
if(posesToro.size() == 0)
{
UERROR("No TORO poses!? (input poses=%d, links=%d, depthGraph=%d)",
(int)poses.size(), (int)links.size(), (int)depthGraph.size());
}
}
}
else if(depthGraph.size() == 1)
{
std::map<int, Transform>::const_iterator iter = poses.find(depthGraph.begin()->first);
if(iter != poses.end())
{
UASSERT_MSG(!iter->second.isNull(), uFormat("Poses should not be null! Id=%d", iter->first).c_str());
optimizedPoses.insert(*iter);
}
else
{
UERROR("Pose %d from depthGraph not found in the poses map!", depthGraph.begin()->first);
}
}
else
{
UERROR("Wrong inputs! depthGraph=%d poses=%d links=%d",
(int)depthGraph.size(), (int)poses.size(), (int)links.size());
}
}
//On success, optimizedPoses is cleared and new poses are inserted in
void optimizeTOROGraph(
//////////////////
// TORO
//////////////////
std::map<int, Transform> TOROOptimizer::optimize(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints,
std::map<int, Transform> & optimizedPoses,
int toroIterations,
bool toroInitialGuess,
bool ignoreCovariance,
std::list<std::map<int, Transform> > * intermediateGraphes) // contains poses after tree init to last one before the end
{
std::map<int, Transform> optimizedPoses;
UDEBUG("Optimizing graph...");
UASSERT(toroIterations>0);
optimizedPoses.clear();
if(edgeConstraints.size()>=1 && poses.size()>=2)
if(edgeConstraints.size()>=1 && poses.size()>=2 && iterations() > 0)
{
// Apply TORO optimization
AISNavigation::TreeOptimizer3 pg;
pg.verboseLevel = 0;
AISNavigation::TreeOptimizer2 pg2;
AISNavigation::TreeOptimizer3 pg3;
pg2.verboseLevel = 0;
pg3.verboseLevel = 0;
UDEBUG("fill poses to TORO...");
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
if(isSlam2d())
{
float x,y,z, roll,pitch,yaw;
UASSERT(!iter->second.isNull());
pcl::getTranslationAndEulerAngles(iter->second.toEigen3f(), x,y,z, roll,pitch,yaw);
AISNavigation::TreePoseGraph3::Pose p(x, y, z, roll, pitch, yaw);
AISNavigation::TreePoseGraph<AISNavigation::Operations3D<double> >::Vertex* v = pg.addVertex(iter->first, p);
if (v)
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
UASSERT(!iter->second.isNull());
AISNavigation::TreePoseGraph2::Pose p(iter->second.x(), iter->second.y(), iter->second.theta());
AISNavigation::TreePoseGraph2::Vertex* v = pg2.addVertex(iter->first, p);
UASSERT_MSG(v != 0, uFormat("cannot insert vertex %d!?", iter->first).c_str());
}
}
else
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
UASSERT(!iter->second.isNull());
float x,y,z, roll,pitch,yaw;
iter->second.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
AISNavigation::TreePoseGraph3::Pose p(x, y, z, roll, pitch, yaw);
AISNavigation::TreePoseGraph3::Vertex* v = pg3.addVertex(iter->first, p);
UASSERT_MSG(v != 0, uFormat("cannot insert vertex %d!?", iter->first).c_str());
v->transformation=AISNavigation::TreePoseGraph3::Transformation(p);
}
else
{
UERROR("cannot insert vertex %d!?", iter->first);
}
}
UDEBUG("fill edges to TORO...");
for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
if(isSlam2d())
{
int id1 = iter->first;
int id2 = iter->second.to();
float x,y,z, roll,pitch,yaw;
UASSERT(!iter->second.transform().isNull());
pcl::getTranslationAndEulerAngles(iter->second.transform().toEigen3f(), x,y,z, roll,pitch,yaw);
AISNavigation::TreePoseGraph3::Pose p(x, y, z, roll, pitch, yaw);
AISNavigation::TreePoseGraph3::InformationMatrix inf = DMatrix<double>::I(6);
if(!ignoreCovariance)
for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
{
if(iter->second.rotVariance()>0)
UASSERT(!iter->second.transform().isNull());
AISNavigation::TreePoseGraph2::Pose p(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta());
AISNavigation::TreePoseGraph2::InformationMatrix inf;
//Identity:
inf.values[0][0] = 1.0f; inf.values[0][1] = 0.0f; inf.values[0][2] = 0.0f; // x
inf.values[1][0] = 0.0f; inf.values[1][1] = 1.0f; inf.values[1][2] = 0.0f; // y
inf.values[2][0] = 0.0f; inf.values[2][1] = 0.0f; inf.values[2][2] = 1.0f; // theta
if(!isCovarianceIgnored())
{
inf[0][0] = 1.0f/iter->second.rotVariance(); // roll
inf[1][1] = 1.0f/iter->second.rotVariance(); // pitch
inf[2][2] = 1.0f/iter->second.rotVariance(); // yaw
if(iter->second.transVariance()>0)
{
inf.values[0][0] = 1.0f/iter->second.transVariance(); // x
inf.values[1][1] = 1.0f/iter->second.transVariance(); // y
}
if(iter->second.rotVariance()>0)
{
inf.values[2][2] = 1.0f/iter->second.rotVariance(); // theta
}
}
if(iter->second.transVariance()>0)
int id1 = iter->first;
int id2 = iter->second.to();
AISNavigation::TreePoseGraph2::Vertex* v1=pg2.vertex(id1);
AISNavigation::TreePoseGraph2::Vertex* v2=pg2.vertex(id2);
AISNavigation::TreePoseGraph2::Transformation t(p);
if (!pg2.addEdge(v1, v2, t, inf))
{
inf[3][3] = 1.0f/iter->second.transVariance(); // x
inf[4][4] = 1.0f/iter->second.transVariance(); // y
inf[5][5] = 1.0f/iter->second.transVariance(); // z
UERROR("Map: Edge already exits between nodes %d and %d, skipping", id1, id2);
}
}
AISNavigation::TreePoseGraph<AISNavigation::Operations3D<double> >::Vertex* v1=pg.vertex(id1);
AISNavigation::TreePoseGraph<AISNavigation::Operations3D<double> >::Vertex* v2=pg.vertex(id2);
AISNavigation::TreePoseGraph3::Transformation t(p);
if (!pg.addEdge(v1, v2, t, inf))
}
else
{
for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
{
UERROR("Map: Edge already exits between nodes %d and %d, skipping", id1, id2);
return;
UASSERT(!iter->second.transform().isNull());
float x,y,z, roll,pitch,yaw;
iter->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
AISNavigation::TreePoseGraph3::Pose p(x, y, z, roll, pitch, yaw);
AISNavigation::TreePoseGraph3::InformationMatrix inf = DMatrix<double>::I(6);
if(!isCovarianceIgnored())
{
if(iter->second.rotVariance()>0)
{
inf[0][0] = 1.0f/iter->second.rotVariance(); // roll
inf[1][1] = 1.0f/iter->second.rotVariance(); // pitch
inf[2][2] = 1.0f/iter->second.rotVariance(); // yaw
}
if(iter->second.transVariance()>0)
{
inf[3][3] = 1.0f/iter->second.transVariance(); // x
inf[4][4] = 1.0f/iter->second.transVariance(); // y
inf[5][5] = 1.0f/iter->second.transVariance(); // z
}
}
int id1 = iter->first;
int id2 = iter->second.to();
AISNavigation::TreePoseGraph3::Vertex* v1=pg3.vertex(id1);
AISNavigation::TreePoseGraph3::Vertex* v2=pg3.vertex(id2);
AISNavigation::TreePoseGraph3::Transformation t(p);
if (!pg3.addEdge(v1, v2, t, inf))
{
UERROR("Map: Edge already exits between nodes %d and %d, skipping", id1, id2);
}
}
}
UDEBUG("buildMST...");
pg.buildMST(pg.vertices.begin()->first); // pg.buildSimpleTree();
UDEBUG("Initial guess...");
if(toroInitialGuess)
UASSERT(uContains(poses, rootId));
if(isSlam2d())
{
pg.initializeOnTree(); // optional
pg2.buildMST(rootId); // pg.buildSimpleTree();
pg2.initializeOnTree();
pg2.initializeTreeParameters();
UDEBUG("Building TORO tree... (if a crash happens just after this msg, "
"TORO is not able to find the root of the graph!)");
pg2.initializeOptimization();
}
else
{
pg3.buildMST(rootId); // pg.buildSimpleTree();
pg3.initializeOnTree();
pg3.initializeTreeParameters();
UDEBUG("Building TORO tree... (if a crash happens just after this msg, "
"TORO is not able to find the root of the graph!)");
pg3.initializeOptimization();
}
pg.initializeTreeParameters();
UDEBUG("Building TORO tree... (if a crash happens just after this msg, "
"TORO is not able to find the root of the graph!)");
pg.initializeOptimization();
UDEBUG("TORO iterate begin (iterations=%d)", toroIterations);
for (int i=0; i<toroIterations; i++)
UINFO("TORO iterate begin (iterations=%d)", iterations());
for (int i=0; i<iterations(); i++)
{
if(intermediateGraphes && (toroInitialGuess || i>0))
if(intermediateGraphes && i>0)
{
std::map<int, Transform> tmpPoses;
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
if(isSlam2d())
{
AISNavigation::TreePoseGraph<AISNavigation::Operations3D<double> >::Vertex* v=pg.vertex(iter->first);
v->pose=v->transformation.toPoseType();
Transform newPose = Transform::fromEigen3f(pcl::getTransformation(v->pose.x(), v->pose.y(), v->pose.z(), v->pose.roll(), v->pose.pitch(), v->pose.yaw()));
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
AISNavigation::TreePoseGraph2::Vertex* v=pg2.vertex(iter->first);
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
Transform newPose(v->pose.x(), v->pose.y(), iter->second.z(), roll, pitch, v->pose.theta());
tmpPoses.insert(std::pair<int, Transform>(iter->first, newPose));
tmpPoses.insert(std::pair<int, Transform>(iter->first, newPose));
}
}
else
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
AISNavigation::TreePoseGraph3::Vertex* v=pg3.vertex(iter->first);
AISNavigation::TreePoseGraph3::Pose pose=v->transformation.toPoseType();
Transform newPose(pose.x(), pose.y(), pose.z(), pose.roll(), pose.pitch(), pose.yaw());
tmpPoses.insert(std::pair<int, Transform>(iter->first, newPose));
}
}
intermediateGraphes->push_back(tmpPoses);
}
if(isSlam2d())
{
pg2.iterate();
pg.iterate();
// compute the error and dump it
double error=pg2.error();
UDEBUG("iteration %d global error=%f error/constraint=%f", i, error, error/pg2.edges.size());
}
else
{
pg3.iterate();
// compute the error and dump it
double mte, mre, are, ate;
double error=pg3.error(&mre, &mte, &are, &ate);
UDEBUG("iteration %d RotGain=%f global error=%f error/constraint=%f mte=%f mre=%f are=%f ate=%f",
i, pg3.getRotGain(), error, error/pg3.edges.size(), mte, mre, are, ate);
}
}
UDEBUG("TORO iterate end");
UINFO("TORO iterate end");
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
if(isSlam2d())
{
AISNavigation::TreePoseGraph<AISNavigation::Operations3D<double> >::Vertex* v=pg.vertex(iter->first);
v->pose=v->transformation.toPoseType();
Transform newPose = Transform::fromEigen3f(pcl::getTransformation(v->pose.x(), v->pose.y(), v->pose.z(), v->pose.roll(), v->pose.pitch(), v->pose.yaw()));
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
AISNavigation::TreePoseGraph2::Vertex* v=pg2.vertex(iter->first);
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
Transform newPose(v->pose.x(), v->pose.y(), iter->second.z(), roll, pitch, v->pose.theta());
optimizedPoses.insert(std::pair<int, Transform>(iter->first, newPose));
optimizedPoses.insert(std::pair<int, Transform>(iter->first, newPose));
}
}
else
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
AISNavigation::TreePoseGraph3::Vertex* v=pg3.vertex(iter->first);
AISNavigation::TreePoseGraph3::Pose pose=v->transformation.toPoseType();
Transform newPose(pose.x(), pose.y(), pose.z(), pose.roll(), pose.pitch(), pose.yaw());
//Eigen::Matrix4f newPose = transformToEigen4f(optimizedPoses.at(poses.rbegin()->first));
//Eigen::Matrix4f oldPose = transformToEigen4f(poses.rbegin()->second);
//Eigen::Matrix4f poseCorrection = oldPose.inverse() * newPose; // transform from odom to correct odom
//Eigen::Matrix4f result = oldPose*poseCorrection*oldPose.inverse();
//mapCorrection = transformFromEigen4f(result);
optimizedPoses.insert(std::pair<int, Transform>(iter->first, newPose));
}
}
}
else if(edgeConstraints.size() == 0 && poses.size() == 1)
else if(poses.size() == 1 || iterations() <= 0)
{
optimizedPoses = poses;
}
@@ -387,9 +413,10 @@ void optimizeTOROGraph(
UWARN("This method should be called at least with 1 pose!");
}
UDEBUG("Optimizing graph...end!");
return optimizedPoses;
}
bool saveTOROGraph(
bool TOROOptimizer::saveGraph(
const std::string & fileName,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints)
@@ -451,7 +478,8 @@ bool saveTOROGraph(
return true;
}
bool loadTOROGraph(const std::string & fileName,
bool TOROOptimizer::loadGraph(
const std::string & fileName,
std::map<int, Transform> & poses,
std::multimap<int, std::pair<int, Transform> > & edgeConstraints)
{
@@ -529,6 +557,312 @@ bool loadTOROGraph(const std::string & fileName,
}
//////////////////////
// g2o
//////////////////////
bool G2OOptimizer::available()
{
#ifdef WITH_G2O
return true;
#else
return false;
#endif
}
std::map<int, Transform> G2OOptimizer::optimize(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints,
std::list<std::map<int, Transform> > * intermediateGraphes)
{
std::map<int, Transform> optimizedPoses;
#ifdef WITH_G2O
UDEBUG("Optimizing graph...");
optimizedPoses.clear();
if(edgeConstraints.size()>=1 && poses.size()>=2 && iterations() > 0)
{
// Apply g2o optimization
// create the linear solver
g2o::BlockSolverX::LinearSolverType * linearSolver = new g2o::LinearSolverCSparse<g2o::BlockSolverX::PoseMatrixType>();
// create the block solver on top of the linear solver
g2o::BlockSolverX* blockSolver = new g2o::BlockSolverX(linearSolver);
// create the algorithm to carry out the optimization
//g2o::OptimizationAlgorithmGaussNewton* optimizationAlgorithm = new g2o::OptimizationAlgorithmGaussNewton(blockSolver);
g2o::OptimizationAlgorithmLevenberg* optimizationAlgorithm = new g2o::OptimizationAlgorithmLevenberg(blockSolver);
// create the optimizer to load the data and carry out the optimization
g2o::SparseOptimizer optimizer;
optimizer.setVerbose(false);
optimizer.setAlgorithm(optimizationAlgorithm);
UDEBUG("fill poses to g2o...");
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
UASSERT(!iter->second.isNull());
g2o::HyperGraph::Vertex * vertex = 0;
if(isSlam2d())
{
g2o::VertexSE2 * v2 = new g2o::VertexSE2();
v2->setEstimate(g2o::SE2(iter->second.x(), iter->second.y(), iter->second.theta()));
vertex = v2;
}
else
{
g2o::VertexSE3 * v3 = new g2o::VertexSE3();
Eigen::Isometry3d pose;
Eigen::Affine3d a = iter->second.toEigen3d();
pose.translation() = a.translation();
pose.linear() = a.rotation();
v3->setEstimate(pose);
vertex = v3;
}
vertex->setId(iter->first);
UASSERT_MSG(optimizer.addVertex(vertex), uFormat("cannot insert vertex %d!?", iter->first).c_str());
}
UDEBUG("fill edges to g2o...");
for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
{
int id1 = iter->first;
int id2 = iter->second.to();
UASSERT(!iter->second.transform().isNull());
g2o::HyperGraph::Edge * edge = 0;
if(isSlam2d())
{
Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
if(!isCovarianceIgnored())
{
if(iter->second.transVariance()>0)
{
information(0,0) = 1.0f/iter->second.transVariance(); // x
information(1,1) = 1.0f/iter->second.transVariance(); // y
}
if(iter->second.rotVariance()>0)
{
information(2,2) = 1.0f/iter->second.rotVariance(); // theta
}
}
g2o::EdgeSE2 * e = new g2o::EdgeSE2();
g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2);
e->setVertex(0, v1);
e->setVertex(1, v2);
e->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()));
e->setInformation(information);
edge = e;
}
else
{
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
if(!isCovarianceIgnored())
{
if(iter->second.transVariance()>0)
{
information(0,0) = 1.0f/iter->second.transVariance(); // x
information(1,1) = 1.0f/iter->second.transVariance(); // y
information(2,2) = 1.0f/iter->second.transVariance(); // z
}
if(iter->second.rotVariance()>0)
{
information(3,3) = 1.0f/iter->second.rotVariance(); // roll
information(4,4) = 1.0f/iter->second.rotVariance(); // pitch
information(5,5) = 1.0f/iter->second.rotVariance(); // yaw
}
}
Eigen::Affine3d a = iter->second.transform().toEigen3d();
Eigen::Isometry3d constraint;
constraint.translation() = a.translation();
constraint.linear() = a.rotation();
g2o::EdgeSE3 * e = new g2o::EdgeSE3();
g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2);
e->setVertex(0, v1);
e->setVertex(1, v2);
e->setMeasurement(constraint);
e->setInformation(information);
edge = e;
}
if (!optimizer.addEdge(edge))
{
delete edge;
UERROR("Map: Failed adding constraint between %d and %d, skipping", id1, id2);
}
}
UDEBUG("Initial optimization...");
UASSERT(uContains(poses, rootId));
if(isSlam2d())
{
g2o::VertexSE2* firstRobotPose = (g2o::VertexSE2*)optimizer.vertex(rootId);
UASSERT(firstRobotPose != 0);
firstRobotPose->setFixed(true);
}
else
{
g2o::VertexSE3* firstRobotPose = (g2o::VertexSE3*)optimizer.vertex(rootId);
UASSERT(firstRobotPose != 0);
firstRobotPose->setFixed(true);
}
UINFO("g2o iterate begin (max iterations=%d)", iterations());
int it = 0;
if(intermediateGraphes)
{
optimizer.initializeOptimization();
for(int i=0; i<iterations(); ++i)
{
if(i > 0)
{
std::map<int, Transform> tmpPoses;
if(isSlam2d())
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
const g2o::VertexSE2* v = (const g2o::VertexSE2*)optimizer.vertex(iter->first);
if(v)
{
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
Transform t(v->estimate().translation()[0], v->estimate().translation()[1], iter->second.z(), roll, pitch, v->estimate().rotation().angle());
tmpPoses.insert(std::pair<int, Transform>(iter->first, t));
}
else
{
UERROR("Vertex %d not found!?", iter->first);
}
}
}
else
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
const g2o::VertexSE3* v = (const g2o::VertexSE3*)optimizer.vertex(iter->first);
if(v)
{
Transform t = Transform::fromEigen3d(v->estimate());
tmpPoses.insert(std::pair<int, Transform>(iter->first, t));
}
else
{
UERROR("Vertex %d not found!?", iter->first);
}
}
}
intermediateGraphes->push_back(tmpPoses);
}
it += optimizer.optimize(1);
if(ULogger::level() == ULogger::kDebug)
{
optimizer.computeActiveErrors();
UDEBUG("iteration %d: %d nodes, %d edges, chi2: %f", i, (int)optimizer.vertices().size(), (int)optimizer.edges().size(), optimizer.chi2());
}
}
}
else
{
optimizer.initializeOptimization();
it = optimizer.optimize(iterations());
optimizer.computeActiveErrors();
UDEBUG("%d nodes, %d edges, chi2: %f", (int)optimizer.vertices().size(), (int)optimizer.edges().size(), optimizer.chi2());
}
UINFO("g2o iterate end (%d iterations done)", it);
if(isSlam2d())
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
const g2o::VertexSE2* v = (const g2o::VertexSE2*)optimizer.vertex(iter->first);
if(v)
{
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
Transform t(v->estimate().translation()[0], v->estimate().translation()[1], iter->second.z(), roll, pitch, v->estimate().rotation().angle());
optimizedPoses.insert(std::pair<int, Transform>(iter->first, t));
}
else
{
UERROR("Vertex %d not found!?", iter->first);
}
}
}
else
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
const g2o::VertexSE3* v = (const g2o::VertexSE3*)optimizer.vertex(iter->first);
if(v)
{
Transform t = Transform::fromEigen3d(v->estimate());
optimizedPoses.insert(std::pair<int, Transform>(iter->first, t));
}
else
{
UERROR("Vertex %d not found!?", iter->first);
}
}
}
optimizer.clear();
g2o::Factory::destroy();
g2o::OptimizationAlgorithmFactory::destroy();
g2o::HyperGraphActionLibrary::destroy();
}
else if(poses.size() == 1 || iterations() <= 0)
{
optimizedPoses = poses;
}
else
{
UWARN("This method should be called at least with 1 pose!");
}
UDEBUG("Optimizing graph...end!");
#else
UERROR("Not built with G2O support!");
#endif
return optimizedPoses;
}
////////////////////////////////////////////
// Graph utilities
////////////////////////////////////////////
std::multimap<int, Link>::iterator findLink(
std::multimap<int, Link> & links,
int from,
int to)
{
std::multimap<int, Link>::iterator iter = links.find(from);
while(iter != links.end() && iter->first == from)
{
if(iter->second.to() == to)
{
return iter;
}
++iter;
}
// let's try to -> from
iter = links.find(to);
while(iter != links.end() && iter->first == to)
{
if(iter->second.to() == from)
{
return iter;
}
++iter;
}
return links.end();
}
std::map<int, Transform> radiusPosesFiltering(
const std::map<int, Transform> & poses,
float radius,
corelib/src/Rtabmap.cpp
+40 -24
View File
@@ -99,8 +99,6 @@ Rtabmap::Rtabmap() :
_localRadius(Parameters::defaultRGBDLocalRadius()),
_localDetectMaxNeighbors(Parameters::defaultRGBDLocalLoopDetectionNeighbors()),
_localDetectMaxDiffID(Parameters::defaultRGBDLocalLoopDetectionMaxDiffID()),
_toroIterations(Parameters::defaultRGBDToroIterations()),
_toroIgnoreVariance(Parameters::defaultRGBDToroIgnoreVariance()),
_databasePath(""),
_optimizeFromGraphEnd(Parameters::defaultRGBDOptimizeFromGraphEnd()),
_reextractLoopClosureFeatures(Parameters::defaultLccReextractActivated()),
@@ -116,6 +114,7 @@ Rtabmap::Rtabmap() :
_lastProcessTime(0.0),
_epipolarGeometry(0),
_bayesFilter(0),
_graphOptimizer(0),
_memory(0),
_foutFloat(0),
_foutInt(0),
@@ -325,6 +324,11 @@ void Rtabmap::close()
delete _bayesFilter;
_bayesFilter = 0;
}
if(_graphOptimizer)
{
delete _graphOptimizer;
_graphOptimizer = 0;
}
_databasePath.clear();
}
@@ -367,8 +371,6 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRGBDLocalRadius(), _localRadius);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionNeighbors(), _localDetectMaxNeighbors);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionMaxDiffID(), _localDetectMaxDiffID);
Parameters::parse(parameters, Parameters::kRGBDToroIterations(), _toroIterations);
Parameters::parse(parameters, Parameters::kRGBDToroIgnoreVariance(), _toroIgnoreVariance);
Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd);
Parameters::parse(parameters, Parameters::kLccReextractActivated(), _reextractLoopClosureFeatures);
Parameters::parse(parameters, Parameters::kLccReextractNNType(), _reextractNNType);
@@ -442,6 +444,29 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
_bayesFilter->parseParameters(parameters);
}
// Graph optimizer
graph::Optimizer::Type optimizerType = graph::Optimizer::kTypeUndef;
if((iter=parameters.find(Parameters::kRGBDOptimizeStrategy())) != parameters.end())
{
optimizerType = (graph::Optimizer::Type)std::atoi((*iter).second.c_str());
}
if(optimizerType!=graph::Optimizer::kTypeUndef)
{
UDEBUG("new detector strategy %d", int(optimizerType));
if(_graphOptimizer)
{
delete _graphOptimizer;
_graphOptimizer = 0;
}
_graphOptimizer = graph::Optimizer::create(optimizerType, parameters);
}
else if(_graphOptimizer)
{
_graphOptimizer->parseParameters(parameters);
}
for(ParametersMap::const_iterator iter = parameters.begin(); iter!=parameters.end(); ++iter)
{
uInsert(_lastParameters, ParametersPair(iter->first, iter->second));
@@ -669,7 +694,7 @@ void Rtabmap::generateTOROGraph(const std::string & path, bool optimized, bool g
_memory->getMetricConstraints(uKeys(ids), poses, constraints, global);
}
rtabmap::graph::saveTOROGraph(path, poses, constraints);
graph::TOROOptimizer::saveGraph(path, poses, constraints);
}
}
@@ -772,7 +797,7 @@ bool Rtabmap::process(const SensorData & data)
timer.start();
timerTotal.start();
if(!_memory || !_bayesFilter)
if(!_memory || !_bayesFilter || !_graphOptimizer)
{
UFATAL("RTAB-Map is not initialized, data received is ignored.");
}
@@ -1469,7 +1494,7 @@ bool Rtabmap::process(const SensorData & data)
_localLoopClosureDetectionSpace &&
!signature->getLaserScanCompressed().empty())
{
if(_toroIterations == 0)
if(_graphOptimizer->iterations() == 0)
{
UWARN("Cannot do local loop closure detection in space if graph optimization is disabled!");
}
@@ -1481,11 +1506,11 @@ bool Rtabmap::process(const SensorData & data)
std::map<int, Transform> localSpacePoses;
localSpaceNearestId = 0;
localSpacePoses = this->getWMPosesInRadius(
signature->id(),
_localDetectMaxNeighbors,
_localRadius,
_localDetectMaxDiffID,
localSpaceNearestId);
signature->id(),
_localDetectMaxNeighbors,
_localRadius,
_localDetectMaxDiffID,
localSpaceNearestId);
// add current node to poses
localSpacePoses.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
@@ -2149,16 +2174,7 @@ void Rtabmap::optimizeCurrentMap(
UDEBUG("get ids=%d", (int)ids.size());
if(!_optimizeFromGraphEnd && ids.size() > 1)
{
UTimer timer;
int first = ids.begin()->first;
ids = _memory->getNeighborsId(first, 0, lookInDatabase?-1:0, true);
UDEBUG("Optimize from the first location (%d) instead of the last (%d) "
"in the local graph. Recomputing neighbors depth... time=%fs",
first,
id,
timer.ticks());
id = ids.begin()->first;
}
UINFO("get ids time %f s", timer.ticks());
@@ -2173,14 +2189,14 @@ void Rtabmap::optimizeCurrentMap(
*constraints = edgeConstraints;
}
if(_toroIterations == 0)
if(_graphOptimizer->iterations() == 0)
{
// Optimization desactivated! Return not optimized poses.
optimizedPoses = poses;
}
else
{
rtabmap::graph::optimizeTOROGraph(ids, poses, edgeConstraints, optimizedPoses, _toroIterations, true, _toroIgnoreVariance);
optimizedPoses = _graphOptimizer->optimize(id, poses, edgeConstraints);
}
UINFO("optimize time %f s", timer.ticks());
}
corelib/src/Transform.cpp
+36
View File
@@ -131,6 +131,13 @@ void Transform::setIdentity()
*this = getIdentity();
}
float Transform::theta() const
{
float roll, pitch, yaw;
this->getEulerAngles(roll, pitch, yaw);
return yaw;
}
Transform Transform::inverse() const
{
return fromEigen4f(toEigen4f().inverse());
@@ -155,6 +162,12 @@ void Transform::getTranslationAndEulerAngles(float & x, float & y, float & z, fl
pcl::getTranslationAndEulerAngles(toEigen3f(), x, y, z, roll, pitch, yaw);
}
void Transform::getEulerAngles(float & roll, float & pitch, float & yaw) const
{
float x,y,z;
pcl::getTranslationAndEulerAngles(toEigen3f(), x, y, z, roll, pitch, yaw);
}
void Transform::getTranslation(float & x, float & y, float & z) const
{
x = this->x();
@@ -252,6 +265,16 @@ Eigen::Affine3d Transform::toEigen3d() const
return Eigen::Affine3d(toEigen4d());
}
Eigen::Quaternionf Transform::getQuaternionf() const
{
return Eigen::Quaternionf(this->toEigen3f().rotation()).normalized();
}
Eigen::Quaterniond Transform::getQuaterniond() const
{
return Eigen::Quaterniond(this->toEigen3d().rotation()).normalized();
}
Transform Transform::getIdentity()
{
return Transform(1,0,0,0, 0,1,0,0, 0,0,1,0);
@@ -283,4 +306,17 @@ Transform Transform::fromEigen3d(const Eigen::Affine3d & matrix)
matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
}
Transform Transform::fromEigen3f(const Eigen::Isometry3f & matrix)
{
return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
}
Transform Transform::fromEigen3d(const Eigen::Isometry3d & matrix)
{
return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
}
}
corelib/src/toro3d/posegraph2.cpp
+441
View File
@@ -0,0 +1,441 @@
/**********************************************************************
*
* This source code is part of the Tree-based Network Optimizer (TORO)
*
* TORO Copyright (c) 2007 Giorgio Grisetti, Cyrill Stachniss,
* Slawomir Grzonka and Wolfram Burgard
*
* TORO is licences under the Common Creative License,
* Attribution-NonCommercial-ShareAlike 3.0
*
* You are free:
* - to Share - to copy, distribute and transmit the work
* - to Remix - to adapt the work
*
* Under the following conditions:
*
* - Attribution. You must attribute the work in the manner specified
* by the author or licensor (but not in any way that suggests that
* they endorse you or your use of the work).
*
* - Noncommercial. You may not use this work for commercial purposes.
*
* - Share Alike. If you alter, transform, or build upon this work,
* you may distribute the resulting work only under the same or
* similar license to this one.
*
* Any of the above conditions can be waived if you get permission
* from the copyright holder. Nothing in this license impairs or
* restricts the author's moral rights.
*
* TORO is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
* PURPOSE.
**********************************************************************/
/** \file posegraph2.cpp
*
* \brief Defines the graph of 2D poses, with specific functionalities
* such as loading, saving, merging constraints, and etc.
**/
#include "posegraph2.hh"
#include <fstream>
#include <sstream>
#include <string>
using namespace std;
namespace AISNavigation {
#define LINESIZE 81920
#define DEBUG(i) \
if (verboseLevel>i) cerr
bool TreePoseGraph2::load(const char* filename, bool overrideCovariances){
clear();
ifstream is(filename);
if (!is)
return false;
while(is){
char buf[LINESIZE];
is.getline(buf,LINESIZE);
istringstream ls(buf);
string tag;
ls >> tag;
if (tag=="VERTEX" || tag=="VERTEX2"){
int id;
Pose p;
ls >> id >> p.x() >> p.y() >> p.theta();
if (addVertex(id,p))
DEBUG(2) << "V " << id << endl;
}
if (tag=="EDGE" || tag=="EDGE2"){
int id1, id2;
Pose p;
InformationMatrix m;
ls >> id1 >> id2 >> p.x() >> p.y() >> p.theta();
if (overrideCovariances){
m.values[0][0]=1; m.values[1][1]=1; m.values[2][2]=1;
m.values[0][1]=0; m.values[0][2]=0; m.values[1][2]=0;
} else {
ls >> m.values[0][0] >> m.values[0][1] >> m.values [1][1]
>> m.values[2][2] >> m.values[0][2] >> m.values [1][2];
}
m.values[1][0]=m.values[0][1];
m.values[2][0]=m.values[0][2];
m.values[2][1]=m.values[1][2];
TreePoseGraph2::Vertex* v1=vertex(id1);
TreePoseGraph2::Vertex* v2=vertex(id2);
Transformation t(p);
if (addEdge(v1, v2,t ,m))
DEBUG(2) << "E " << id1 << " " << id2 << endl;
}
}
return true;
}
bool TreePoseGraph2::loadEquivalences(const char* filename){
ifstream is(filename);
if (!is)
return false;
EdgeList suppressed;
uint equivCount=0;
while (is){
char buf[LINESIZE];
is.getline(buf, LINESIZE);
istringstream ls(buf);
string tag;
ls >> tag;
if (tag=="EQUIV"){
int id1, id2;
ls >> id1 >> id2;
Edge* e=edge(id1,id2);
if (!e)
e=edge(id2,id1);
if (e){
suppressed.push_back(e);
equivCount++;
}
}
}
for (EdgeList::iterator it=suppressed.begin(); it!=suppressed.end(); it++){
Edge* e=*it;
if (e->v1->id > e->v2->id)
revertEdge(e);
collapseEdge(e);
}
for (TreePoseGraph2::VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
Vertex* v=it->second;
v->edges.clear();
}
for (TreePoseGraph2::EdgeMap::iterator it=edges.begin(); it!=edges.end(); it++){
TreePoseGraph2::Edge * e=it->second;
e->v1->edges.push_back(e);
e->v2->edges.push_back(e);
}
return true;
}
bool TreePoseGraph2::saveGnuplot(const char* filename){
ofstream os(filename);
if (!os)
return false;
for (TreePoseGraph2::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
const TreePoseGraph2::Edge * e=it->second;
const Vertex* v1=e->v1;
const Vertex* v2=e->v2;
os << v1->pose.x() << " " << v1->pose.y() << " " << v1->pose.theta() << endl;
os << v2->pose.x() << " " << v2->pose.y() << " " << v2->pose.theta() << endl;
os << endl;
}
return true;
}
bool TreePoseGraph2::save(const char* filename){
ofstream os(filename);
if (!os)
return false;
for (TreePoseGraph2::VertexMap::const_iterator it=vertices.begin(); it!=vertices.end(); it++){
const TreePoseGraph2::Vertex* v=it->second;
os << "VERTEX "
<< v->id << " "
<< v->pose.x() << " "
<< v->pose.y() << " "
<< v->pose.theta()<< endl;
}
for (TreePoseGraph2::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
const TreePoseGraph2::Edge * e=it->second;
os << "EDGE " << e->v1->id << " " << e->v2->id << " ";
Pose p=e->transformation.toPoseType();
os << p.x() << " " << p.y() << " " << p.theta() << " ";
os << e->informationMatrix.values[0][0] << " "
<< e->informationMatrix.values[0][1] << " "
<< e->informationMatrix.values[1][1] << " "
<< e->informationMatrix.values[2][2] << " "
<< e->informationMatrix.values[0][2] << " "
<< e->informationMatrix.values[1][2] << endl;
}
return true;
}
/** \brief A class (struct) used to print vertex information to a
stream. Needed for debugging. **/
struct IdPrinter{
IdPrinter(std::ostream& _os):os(_os){}
std::ostream& os;
void perform(TreePoseGraph2::Vertex* v){
std::cout << "(" << v->id << "," << v->level << ")" << endl;
}
};
void TreePoseGraph2::printDepth( std::ostream& os ){
IdPrinter ip(os);
treeDepthVisit(ip, root);
}
void TreePoseGraph2::printWidth( std::ostream& os ){
IdPrinter ip(os);
treeBreadthVisit(ip);
}
/** \brief A class (struct) for realizing the pose update of the
individual nodes. Assumes the correct order of constraint updates
(according to the tree level, see RSS07 paper)**/
struct PosePropagator{
void perform(TreePoseGraph2::Vertex* v){
if (!v->parent)
return;
TreePoseGraph2::Transformation tParent(v->parent->pose);
TreePoseGraph2::Transformation tNode=tParent*v->parentEdge->transformation;
//cerr << "EDGE(" << v->parentEdge->v1->id << "," << v->parentEdge->v2->id <<"): " << endl;
//Pose pParent=v->parent->pose;
//cerr << " p=" << pParent.x() << "," << pParent.y() << "," << pParent.theta() << endl;
//Pose pEdge=v->parentEdge->transformation.toPoseType();
//cerr << " m=" << pEdge.x() << "," << pEdge.y() << "," << pEdge.theta() << endl;
//Pose pNode=tNode.toPoseType();
//cerr << " n=" << pNode.x() << "," << pNode.y() << "," << pNode.theta() << endl;
assert(v->parentEdge->v1==v->parent);
assert(v->parentEdge->v2==v);
v->pose=tNode.toPoseType();
}
};
void TreePoseGraph2::initializeOnTree(){
PosePropagator pp;
treeDepthVisit(pp, root);
}
void TreePoseGraph2::printEdgesStat(std::ostream& os){
for (TreePoseGraph2::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
const TreePoseGraph2::Edge * e=it->second;
os << "EDGE " << e->v1->id << " " << e->v2->id << " ";
Pose p=e->transformation.toPoseType();
os << p.x() << " " << p.y() << " " << p.theta() << " ";
os << e->informationMatrix.values[0][0] << " "
<< e->informationMatrix.values[0][1] << " "
<< e->informationMatrix.values[1][1] << " "
<< e->informationMatrix.values[2][2] << " "
<< e->informationMatrix.values[0][2] << " "
<< e->informationMatrix.values[1][2] << endl;
os << " top=" << e->top->id << " length=" << e->length << endl;
}
}
void TreePoseGraph2::revertEdgeInfo(Edge* e){
Transformation it=e->transformation.inv();
InformationMatrix R;
R.values[0][0]=e->transformation.rotationMatrix[0][0];
R.values[0][1]=e->transformation.rotationMatrix[0][1];
R.values[0][2]=0;
R.values[1][0]=e->transformation.rotationMatrix[1][0];
R.values[1][1]=e->transformation.rotationMatrix[1][1];
R.values[1][2]=0;
R.values[2][0]=0;
R.values[2][1]=0;
R.values[2][2]=1;
InformationMatrix IM=R.transpose()*e->informationMatrix*R;
Pose np=e->transformation.toPoseType();
Pose ip=it.toPoseType();
Transformation tc=it*e->transformation;
Pose pc=tc.toPoseType();
e->transformation=it;
e->informationMatrix=IM;
};
void TreePoseGraph2::initializeFromParentEdge(Vertex* v){
Transformation tp=Transformation(v->parent->pose)*v->parentEdge->transformation;
v->transformation=tp;
v->pose=tp.toPoseType();
v->parameters=v->pose;
v->parameters.x()-=v->parent->pose.x();
v->parameters.y()-=v->parent->pose.y();
v->parameters.theta()-=v->parent->pose.theta();
v->parameters.theta()=atan2(sin(v->parameters.theta()), cos(v->parameters.theta()));
}
void TreePoseGraph2::collapseEdge(Edge* e){
EdgeMap::iterator ie_it=edges.find(e);
if (ie_it==edges.end())
return;
VertexMap::iterator it1=vertices.find(e->v1->id);
VertexMap::iterator it2=vertices.find(e->v2->id);
assert(it1!=vertices.end());
assert(it2!=vertices.end());
Vertex* v1=e->v1;
Vertex* v2=e->v2;
// all the edges of v2 become outgoing
for (EdgeList::iterator it=v2->edges.begin(); it!=v2->edges.end(); it++){
if ( (*it)->v1!=v2 )
revertEdge(*it);
}
// all the edges of v1 become outgoing
for (EdgeList::iterator it=v1->edges.begin(); it!=v1->edges.end(); it++){
if ( (*it)->v1!=v1 )
revertEdge(*it);
}
assert(e->v1==v1);
InformationMatrix I12=e->informationMatrix;
CovarianceMatrix C12=I12.inv();
Transformation T12=e->transformation;
Pose p12=T12.toPoseType();
Transformation iT12=T12.inv();
//compute the marginal information of the nodes in the path v1-v2-v*
for (EdgeList::iterator it2=v2->edges.begin(); it2!=v2->edges.end(); it2++){
Edge* e2=*it2;
if (e2->v1==v2){ //edge leaving v2
Transformation T2x=e2->transformation;
Pose p2x=T2x.toPoseType();
InformationMatrix I2x=e2->informationMatrix;
CovarianceMatrix C2x=I2x.inv();
//compute the estimate of the vertex based on the path v1-v2-vx
Transformation tr=iT12*T2x;
InformationMatrix R;
R.values[0][0]=tr.rotationMatrix[0][0];
R.values[0][1]=tr.rotationMatrix[0][1];
R.values[0][2]=0;
R.values[1][0]=tr.rotationMatrix[1][0];
R.values[1][1]=tr.rotationMatrix[1][1];
R.values[1][2]=0;
R.values[2][0]=0;
R.values[2][1]=0;
R.values[2][2]=1;
CovarianceMatrix CM=R.transpose()*C2x*R;
Transformation T1x_pred=T12*e2->transformation;
Covariance C1x_pred=C12+C2x;
InformationMatrix I1x_pred=C1x_pred.inv();
e2->transformation=T1x_pred;
e2->informationMatrix=I1x_pred;
}
}
//all the edges leaving v1 and leaving v2 and leading to the same point are merged
std::list<Transformation> tList;
std::list<InformationMatrix> iList;
std::list<Vertex*> vList;
//others are transformed and added to v1
for (EdgeList::iterator it2=v2->edges.begin(); it2!=v2->edges.end(); it2++){
Edge* e1x=0;
Edge* e2x=0;
if ( ((*it2)->v1!=v1)){
e2x=*it2;
for (EdgeList::iterator it1=v1->edges.begin(); it1!=v1->edges.end(); it1++){
if ((*it1)->v2==(*it2)->v2)
e1x=*it1;
}
}
if (e1x && e2x){
Transformation t1x=e1x->transformation;
InformationMatrix I1x=e1x->informationMatrix;
Pose p1x=t1x.toPoseType();
Transformation t2x=e2x->transformation;
InformationMatrix I2x=e2x->informationMatrix;;
Pose p2x=t2x.toPoseType();
InformationMatrix IM=I1x+I2x;
CovarianceMatrix CM=IM.inv();
InformationMatrix scale1=CM*I1x;
InformationMatrix scale2=CM*I2x;
Pose p1=scale1*p1x;
Pose p2=scale2*p2x;
//need to recover the angles in a decent way.
double s=scale1.values[2][2]*sin(p1x.theta())+ scale2.values[2][2]*sin(p2x.theta());
double c=scale1.values[2][2]*cos(p1x.theta())+ scale2.values[2][2]*cos(p2x.theta());
DEBUG(2) << "p1x= " << p1x.x() << " " << p1x.y() << " " << p1x.theta() << endl;
DEBUG(2) << "p1x_pred= " << p2x.x() << " " << p2x.y() << " " << p2x.theta() << endl;
Pose pFinal(p1.x()+p2.x(), p1.y()+p2.y(), atan2(s,c));
DEBUG(2) << "p1x_final= " << pFinal.x() << " " << pFinal.y() << " " << pFinal.theta() << endl;
e1x->transformation=Transformation(pFinal);
e1x->informationMatrix=IM;
}
if (!e1x && e2x){
tList.push_back(e2x->transformation);
iList.push_back(e2x->informationMatrix);
vList.push_back(e2x->v2);
}
}
removeVertex(v2->id);
std::list<Transformation>::iterator t=tList.begin();
std::list<InformationMatrix>::iterator i=iList.begin();
std::list<Vertex*>::iterator v=vList.begin();
while (i!=iList.end()){
addEdge(v1,*v,*t,*i);
i++;
t++;
v++;
}
}
}; //namespace AISNavigation
corelib/src/toro3d/posegraph2.hh
+110
View File
@@ -0,0 +1,110 @@
/**********************************************************************
*
* This source code is part of the Tree-based Network Optimizer (TORO)
*
* TORO Copyright (c) 2007 Giorgio Grisetti, Cyrill Stachniss,
* Slawomir Grzonka and Wolfram Burgard
*
* TORO is licences under the Common Creative License,
* Attribution-NonCommercial-ShareAlike 3.0
*
* You are free:
* - to Share - to copy, distribute and transmit the work
* - to Remix - to adapt the work
*
* Under the following conditions:
*
* - Attribution. You must attribute the work in the manner specified
* by the author or licensor (but not in any way that suggests that
* they endorse you or your use of the work).
*
* - Noncommercial. You may not use this work for commercial purposes.
*
* - Share Alike. If you alter, transform, or build upon this work,
* you may distribute the resulting work only under the same or
* similar license to this one.
*
* Any of the above conditions can be waived if you get permission
* from the copyright holder. Nothing in this license impairs or
* restricts the author's moral rights.
*
* TORO is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
* PURPOSE.
**********************************************************************/
/** \file posegraph2.hh
*
* \brief Defines the graph of 2D poses, with specific functionalities
* such as loading, saving, merging constraints, and etc.
**/
#ifndef _POSEGRAPH2_HH_
#define _POSEGRAPH2_HH_
#include "posegraph.hh"
#include "transformation2.hh"
#include <iostream>
#include <vector>
namespace AISNavigation {
/** \brief The class (struct) that contains 2D graph related functions
such as loading, saving, merging, etc. **/
struct TreePoseGraph2: public TreePoseGraph< Operations2D<double> >{
typedef Operations2D<double>::PoseType Pose;
typedef Operations2D<double>::RotationType Rotation;
typedef Operations2D<double>::TranslationType Translation;
typedef Operations2D<double>::TransformationType Transformation;
typedef Operations2D<double>::CovarianceType CovarianceMatrix;
typedef Operations2D<double>::InformationType InformationMatrix;
/** Load a graph from a file ignoring the equivalence constraints
@param filename the graph file
@param overrideCovariances ignore the covariances from the file, and use identities instead
**/
bool load( const char* filename, bool overrideCovariances=false);
/** Load only the equivalence constraints from a graph file (call load before) **/
bool loadEquivalences( const char* filename);
/** Saves the graph in the graph-format**/
bool save( const char* filename);
/** Saved the graph for visualizing it using gnuplot **/
bool saveGnuplot( const char* filename);
/** Debug function **/
void printDepth( std::ostream& os );
/** Debug function **/
void printWidth( std::ostream& os );
/** Debug function **/
void printEdgesStat( std::ostream& os);
void initializeOnTree();
/** Turn around the edge (<i,j> => <j,i>) **/
virtual void revertEdgeInfo(Edge* e);
virtual void initializeFromParentEdge(Vertex* v);
/** Function to compress a graph. Needed if, for example, equivalence
constraints are used to build a graoh structure with indices
without gaps. **/
virtual void collapseEdge(Edge* e);
/** Specifies the verbose level for debugging **/
int verboseLevel;
};
}; //namespace AISNavigation
#endif
corelib/src/toro3d/transformation2.hh
+410
View File
@@ -0,0 +1,410 @@
/**********************************************************************
*
* This source code is part of the Tree-based Network Optimizer (TORO)
*
* TORO Copyright (c) 2007 Giorgio Grisetti, Cyrill Stachniss,
* Slawomir Grzonka, and Wolfram Burgard
*
* TORO is licences under the Common Creative License,
* Attribution-NonCommercial-ShareAlike 3.0
*
* You are free:
* - to Share - to copy, distribute and transmit the work
* - to Remix - to adapt the work
*
* Under the following conditions:
*
* - Attribution. You must attribute the work in the manner specified
* by the author or licensor (but not in any way that suggests that
* they endorse you or your use of the work).
*
* - Noncommercial. You may not use this work for commercial purposes.
*
* - Share Alike. If you alter, transform, or build upon this work,
* you may distribute the resulting work only under the same or
* similar license to this one.
*
* Any of the above conditions can be waived if you get permission
* from the copyright holder. Nothing in this license impairs or
* restricts the author's moral rights.
*
* TORO is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
* PURPOSE.
**********************************************************************/
/** \file transformation2.hh
* \brief Definition of the 2d transformations.
*
* Definition of the 2d transformations, the symmetrix matrix operations,
* handling covariance, etc.
**/
#ifndef _TRANSFORMATION2_HXX_
#define _TRANSFORMATION2_HXX_
#include <cmath>
namespace AISNavigation
{
/** \brief Template class for representing a 2D point (x and y coordinate) **/
template <class T>
struct Vector2{
T values[2] ; ///< container for x and y
/** Constructor **/
Vector2(T x, T y) {values[0]=x; values[1]=y;}
/** Default constructor which sets x and y to 0 **/
Vector2() {values[0]=0; values[1]=0;}
/** @returns Const reference to x **/
inline const T& x() const {return values[0];}
/** @returns Const reference to y **/
inline const T& y() const {return values[1];}
/** @returns Reference to x **/
inline T& x() {return values[0];}
/** @returns Reference to y **/
inline T& y() {return values[1];}
/** @returns Norm of the vector **/
inline T norm2() const {
return values[0]*values[0]+values[1]*values[1];
}
};
/** Operator for scalar multiplication. **/
template <class T>
inline Vector2<T> operator * (const T& d, const Vector2<T>& v) {
return Vector2<T>(v.values[0]*d, v.values[1]*d);
}
/** Operator for scalar multiplication. **/
template <class T>
inline Vector2<T> operator * (const Vector2<T>& v, const T& d) {
return Vector2<T>(v.values[0]*d, v.values[1]*d);
}
/** Operator for dot product. **/
template <class T>
inline T operator * (const Vector2<T>& v1, const Vector2<T>& v2){
return v1.values[0]*v2.values[0]
+ v1.values[1]*v2.values[1];
}
/** Operator for vector addition. **/
template <class T>
inline Vector2<T> operator + (const Vector2<T>& v1, const Vector2<T>& v2){
return Vector2<T>(v1.values[0]+v2.values[0],
v1.values[1]+v2.values[1]);
}
/** Operator for vector subtraction. **/
template <class T>
Vector2<T> operator - (const Vector2<T>& v1, const Vector2<T>& v2){
return Vector2<T>(v1.values[0]-v2.values[0],
v1.values[1]-v2.values[1]);
}
/** \brief 2D Point (x,y) with orientation (theta)
*
* Tenmplate class for representing a 2D Ooint with x and y
* coordinates and an orientation theta in the x-y-plane (theta=0 ->
* orientation along the x axis).
**/
template <class T>
struct Pose2{
T values[3];///< container for x, y, and theta
/** @returns Const refernce to x **/
inline const T& x() const {return values[0];}
/** @returns Const refernce to y **/
inline const T& y() const {return values[1];}
/** @returns Const refernce to theta **/
inline const T& theta() const {return values[2];}
/** @returns Refernce to x **/
inline T& x() {return values[0];}
/** @returns Refernce to y **/
inline T& y() {return values[1];}
/** @returns Refernce to theta **/
inline T& theta() {return values[2];}
/** Default constructor which sets x, y, and theta to 0 **/
Pose2(){
values[0]=0.; values[1]=0.; values[2]=0.;
}
/** Constructor **/
Pose2(const T& x, const T& y, const T& theta){
values[0]=x, values[1]=y, values[2]=theta;
}
};
/** Operator for scalar multiplication with a pose **/
template <class T>
Pose2<T> operator * (const Pose2<T>& v, const T& d){
Pose2<T> r;
for (int i=0; i<3; i++){
r.values[i]=v.values[i]*d;
}
return r;
}
/** \brief A class to represent 2D transformations (rotation and translation) **/
template <class T>
struct Transformation2{
T rotationMatrix[2][2]; ///< the rotation matrix
T translationVector[2]; ///< the translation vector
/** Default constructor
* @param initAsIdentity if true (default) the transormation
* is the identity, otherwise no initializtion **/
Transformation2(bool initAsIdentity = true){
if (initAsIdentity) {
rotationMatrix[0][0]=1.; rotationMatrix[0][1]=0.;
rotationMatrix[1][0]=0.; rotationMatrix[1][1]=1.;
translationVector[0]=0.;
translationVector[1]=0.;
}
}
/** @returns Identity transformation **/
inline static Transformation2<T> identity(){
Transformation2<T> m(true);
return m;
}
/** Constructor that sets the translation and rotation **/
Transformation2 (const T& x, const T& y, const T& theta){
setRotation(theta);
setTranslation(x,y);
}
/** Constructor that sets the translation and rotation **/
Transformation2 (const T& _theta, const Vector2<T>& trans):
Transformation2(trans.x(), trans.y(), _theta){}
/** Copy constructor **/
Transformation2 (const Pose2<T>& v){
setRotation(v.theta());
setTranslation(v.x(),v.y());
}
/** Get the translation **/
inline Vector2<T> translation() const {
return Vector2<T>(translationVector[0],
translationVector[1]);
}
/** Get the rotation **/
inline T rotation() const {
return atan2(rotationMatrix[1][0],rotationMatrix[0][0]);
}
/** Computed the Pose based on the translation and rotation **/
inline Pose2<T> toPoseType() const {
Vector2<T> t=translation();
T r=rotation();
Pose2<T> rv(t.x(), t.y(), r );
return rv;
}
/** Set the translation **/
inline void setTranslation(const Vector2<T>& t){
setTranslation(t.x(),t.y());
}
/** Set the rotation **/
inline void setRotation(const T& theta){
T s=sin(theta), c=cos(theta);
rotationMatrix[0][0]=c, rotationMatrix[0][1]=-s;
rotationMatrix[1][0]=s, rotationMatrix[1][1]= c;
}
/** Set the translation **/
inline void setTranslation(const T& x, const T& y){
translationVector[0]=x;
translationVector[1]=y;
}
/** Computes the inveres of the transformation **/
inline Transformation2<T> inv() const {
Transformation2<T> rv(*this);
for (int i=0; i<2; i++)
for (int j=0; j<2; j++){
rv.rotationMatrix[i][j]=rotationMatrix[j][i];
}
for (int i=0; i<2; i++){
rv.translationVector[i]=0;
for (int j=0; j<2; j++){
rv.translationVector[i]-=rv.rotationMatrix[i][j]*translationVector[j];
}
}
return rv;
}
};
/** Operator for transforming a Vector2 **/
template <class T>
Vector2<T> operator * (const Transformation2<T>& m, const Vector2<T>& v){
return Vector2<T>(
m.rotationMatrix[0][0]*v.values[0]+
m.rotationMatrix[0][1]*v.values[1]+
m.translationVector[0],
m.rotationMatrix[1][0]*v.values[0]+
m.rotationMatrix[1][1]*v.values[1]+
m.translationVector[1]);
}
/** Operator for concatenating two transformations **/
template <class T>
Transformation2<T> operator * (const Transformation2<T>& m1, const Transformation2<T>& m2){
Transformation2<T> rt;
for (int i=0; i<2; i++)
for (int j=0; j<2; j++){
rt.rotationMatrix[i][j]=0.;
for (int k=0; k<2; k++)
rt.rotationMatrix[i][j]+=m1.rotationMatrix[i][k]*m2.rotationMatrix[k][j];
}
for (int i=0; i<2; i++){
rt.translationVector[i]=m1.translationVector[i];
for (int j=0; j<2; j++)
rt.translationVector[i]+=m1.rotationMatrix[i][j]*m2.translationVector[j];
}
return rt;
}
/** \brief A class to represent symmetric 3x3 matrices **/
template <class T>
struct SMatrix3{
T values[3][3];
T det() const;
SMatrix3<T> transpose() const;
SMatrix3<T> adj() const;
SMatrix3<T> inv() const;
};
/** Operator for symmetric matrix-pose multiplication **/
template <class T>
Pose2<T> operator * (const SMatrix3<T>& m, const Pose2<T>& p){
Pose2<T> v;
for (int i=0; i<3; i++){
v.values[i]=0.;
for (int j=0; j<3; j++)
v.values[i]+=m.values[i][j]*p.values[j];
}
return v;
}
/** Operator for symmetric matrix-scalar multiplication **/
template <class T>
SMatrix3<T> operator * (const SMatrix3<T>& s, T& d){
SMatrix3<T> m;
for (int i=0; i<3; i++)
for (int j=0; j<3; j++)
m.values[i][j]=d*s.values[i][j];
return m;
}
/** Operator forsymmetric matrix-symmetric matrix multiplication **/
template <class T>
SMatrix3<T> operator * (const SMatrix3<T>& s1, const SMatrix3<T>& s2){
SMatrix3<T> m;
for (int i=0; i<3; i++)
for (int j=0; j<3; j++){
m.values[i][j]=0.;
for (int k=0; k<3; k++){
m.values[i][j]+=s1.values[i][k]*s2.values[k][j];
}
}
return m;
}
/** Operator for symmetric matrix-symmetric matrix addition **/
template <class T>
SMatrix3<T> operator + (const SMatrix3<T>& s1, const SMatrix3<T>& s2){
SMatrix3<T> m;
for (int i=0; i<3; i++)
for (int j=0; j<3; j++){
m.values[i][j]=s1.values[i][j]+s2.values[i][j];
}
return m;
}
/** Computes the determinat of the symmetric matrix **/
template <class T>
T SMatrix3<T>::det() const{
T dp= values[0][0]*values[1][1]*values[2][2]
+values[0][1]*values[1][2]*values[2][0]
+values[0][2]*values[1][0]*values[2][1];
T dm=values[2][0]*values[1][1]*values[0][2]
+values[2][1]*values[1][2]*values[0][0]
+values[2][2]*values[1][0]*values[0][1];
return dp-dm;
}
/** Computes the transposed symmetric matrix **/
template <class T>
SMatrix3<T> SMatrix3<T>::transpose() const{
SMatrix3<T> m;
for (int i=0; i<3; i++)
for (int j=0; j<3; j++)
m.values[j][i]=values[i][j];
return m;
}
/** Computes the complement of the symmetric matrix **/
template <class T>
SMatrix3<T> SMatrix3<T>::adj() const{
SMatrix3<T> m;
m.values[0][0]= values[1][1]*values[2][2]-values[2][1]*values[1][2];
m.values[0][1]=-values[1][0]*values[2][2]+values[1][2]*values[2][0];
m.values[0][2]= values[1][0]*values[2][1]-values[2][0]*values[1][1];
m.values[1][0]=-values[0][1]*values[2][2]+values[2][1]*values[0][2];
m.values[1][1]= values[0][0]*values[2][2]-values[2][0]*values[0][2];
m.values[1][2]=-values[0][0]*values[2][1]+values[2][0]*values[0][1];
m.values[2][0]= values[0][1]*values[1][2]-values[1][1]*values[0][2];
m.values[2][1]=-values[0][0]*values[1][2]+values[1][0]*values[0][2];
m.values[2][2]= values[0][0]*values[1][1]-values[1][0]*values[0][1];
return m;
}
/** Computes the inverse (=transposed) symmetric matrix **/
template <class T>
SMatrix3<T> SMatrix3<T>::inv() const{
T id=1./det();
SMatrix3<T> i=adj().transpose();
return i*id;
}
/** \brief Tenmplate class to define the operations in 2D **/
template <class T>
struct Operations2D{
typedef T BaseType; /**< base type of the operation typedef **/
typedef Pose2<T> PoseType; /**< plain representation of the 2d pose as x,y,theta **/
typedef Pose2<T> ParametersType; /**< plain representation of the 2d pose as x,y,theta **/
typedef T RotationType; /**< plain representation of the angle **/
typedef Vector2<T> TranslationType; /**< plain representation of the 2D translation (x,y) **/
typedef Transformation2<T> TransformationType; /**< homogeneous based representation for a 2d pose, as rotation matrix + vector **/
typedef SMatrix3<T> CovarianceType; /**< 3 by 3 symmetric covariance matrix for the 2D case **/
typedef SMatrix3<T> InformationType; /**< 3 by 3 symmetric information matrix for the 2D case **/
};
} // namespace AISNavigation
#endif
corelib/src/toro3d/treeoptimizer2.cpp
+366
View File
@@ -0,0 +1,366 @@
/**********************************************************************
*
* This source code is part of the Tree-based Network Optimizer (TORO)
*
* TORO Copyright (c) 2007 Giorgio Grisetti, Cyrill Stachniss,
* Slawomir Grzonka, and Wolfram Burgard
*
* TORO is licences under the Common Creative License,
* Attribution-NonCommercial-ShareAlike 3.0
*
* You are free:
* - to Share - to copy, distribute and transmit the work
* - to Remix - to adapt the work
*
* Under the following conditions:
*
* - Attribution. You must attribute the work in the manner specified
* by the author or licensor (but not in any way that suggests that
* they endorse you or your use of the work).
*
* - Noncommercial. You may not use this work for commercial purposes.
*
* - Share Alike. If you alter, transform, or build upon this work,
* you may distribute the resulting work only under the same or
* similar license to this one.
*
* Any of the above conditions can be waived if you get permission
* from the copyright holder. Nothing in this license impairs or
* restricts the author's moral rights.
*
* TORO is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
* PURPOSE.
**********************************************************************/
/** \file treeoptimizer2.cpp
*
* \brief Defines the core optimizer class for 2D graphs which is a
* subclass of TreePoseGraph2
*
**/
#include "treeoptimizer2.hh"
#include <fstream>
#include <sstream>
#include <string>
using namespace std;
namespace AISNavigation {
#define DEBUG(i) \
if (verboseLevel>i) cerr
/** \brief A class (struct) to compute the parameterization of the vertex v **/
struct ParameterPropagator{
void perform(TreePoseGraph2::Vertex* v){
if (!v->parent){
v->parameters=TreePoseGraph2::Pose(0.,0.,0.);
return;
}
v->parameters=TreePoseGraph2::Pose(v->pose.x()-v->parent->pose.x(),
v->pose.y()-v->parent->pose.y(),
v->pose.theta()-v->parent->pose.theta());
}
};
TreeOptimizer2::TreeOptimizer2(){
sortedEdges=0;
}
TreeOptimizer2::~TreeOptimizer2(){
}
void TreeOptimizer2::initializeTreeParameters(){
ParameterPropagator pp;
treeDepthVisit(pp, root);
}
void TreeOptimizer2::initializeOptimization(){
// compute the size of the preconditioning matrix
int sz=maxIndex()+1;
DEBUG(1) << "Size= " << sz << endl;
M.resize(sz);
DEBUG(1) << "allocating M(" << sz << ")" << endl;
iteration=1;
// sorting edges
if (sortedEdges!=0){
delete sortedEdges;
sortedEdges=0;
}
sortedEdges=sortEdges();
}
void TreeOptimizer2::initializeOnlineOptimization(){
// compute the size of the preconditioning matrix
int sz=maxIndex()+1;
DEBUG(1) << "Size= " << sz << endl;
M.resize(sz);
DEBUG(1) << "allocating M(" << sz << ")" << endl;
iteration=1;
}
void TreeOptimizer2::computePreconditioner(){
gamma[0] = gamma[1] = gamma[2] = numeric_limits<double>::max();
for (uint i=0; i<M.size(); i++)
M[i]=Pose(0.,0.,0.);
int edgeCount=0;
for (EdgeSet::iterator it=sortedEdges->begin(); it!=sortedEdges->end(); it++){
edgeCount++;
if (! (edgeCount%10000))
DEBUG(1) << "m";
Edge* e=*it;
Transformation t=e->transformation;
InformationMatrix S=e->informationMatrix;
InformationMatrix R;
R.values[0][0]=t.rotationMatrix[0][0];
R.values[0][1]=t.rotationMatrix[0][1];
R.values[0][2]=0;
R.values[1][0]=t.rotationMatrix[1][0];
R.values[1][1]=t.rotationMatrix[1][1];
R.values[1][2]=0;
R.values[2][0]=0;
R.values[2][1]=0;
R.values[2][2]=1;
InformationMatrix W =R*S*R.transpose();
Vertex* top=e->top;
for (int dir=0; dir<2; dir++){
Vertex* n = (dir==0)? e->v1 : e->v2;
while (n!=top){
uint i=n->id;
M[i].values[0]+=W.values[0][0];
M[i].values[1]+=W.values[1][1];
M[i].values[2]+=W.values[2][2];
gamma[0]=gamma[0]<W.values[0][0]?gamma[0]:W.values[0][0];
gamma[1]=gamma[1]<W.values[1][1]?gamma[1]:W.values[1][1];
gamma[2]=gamma[2]<W.values[2][2]?gamma[2]:W.values[2][2];
n=n->parent;
}
}
}
if (verboseLevel>1){
for (uint i=0; i<M.size(); i++){
cerr << "M[" << i << "]=" << M[i].x() << " " << M[i].y() << " " << M[i].theta() <<endl;
}
}
}
void TreeOptimizer2::propagateErrors(){
iteration++;
int edgeCount=0;
for (EdgeSet::iterator it=sortedEdges->begin(); it!=sortedEdges->end(); it++){
edgeCount++;
if (! (edgeCount%10000)) DEBUG(1) << "c";
Edge* e=*it;
Vertex* top=e->top;
Vertex* v1=e->v1;
Vertex* v2=e->v2;
double l=e->length;
DEBUG(2) << "Edge: " << v1->id << " " << v2->id << ", top=" << top->id << ", length="<< l <<endl;
Pose p1=getPose(v1, top);
Pose p2=getPose(v2, top);
DEBUG(2) << " p1=" << p1.x() << " " << p1.y() << " " << p1.theta() << endl;
DEBUG(2) << " p2=" << p2.x() << " " << p2.y() << " " << p2.theta() << endl;
Transformation et=e->transformation;
Transformation t1(p1);
Transformation t2(p2);
Transformation t12=t1*et;
Pose p12=t12.toPoseType();
DEBUG(2) << " pt2=" << p12.x() << " " << p12.y() << " " << p12.theta() << endl;
Pose r(p12.x()-p2.x(), p12.y()-p2.y(), p12.theta()-p2.theta());
double angle=r.theta();
angle=atan2(sin(angle),cos(angle));
r.theta()=angle;
DEBUG(2) << " e=" << r.x() << " " << r.y() << " " << r.theta() << endl;
InformationMatrix S=e->informationMatrix;
InformationMatrix R;
R.values[0][0]=t1.rotationMatrix[0][0];
R.values[0][1]=t1.rotationMatrix[0][1];
R.values[0][2]=0;
R.values[1][0]=t1.rotationMatrix[1][0];
R.values[1][1]=t1.rotationMatrix[1][1];
R.values[1][2]=0;
R.values[2][0]=0;
R.values[2][1]=0;
R.values[2][2]=1;
InformationMatrix W=R*S*R.transpose();
Pose d=W*r*2.;
DEBUG(2) << " d=" << d.x() << " " << d.y() << " " << d.theta() << endl;
assert(l>0);
double alpha[3] = { 1./(gamma[0]*iteration), 1./(gamma[1]*iteration), 1./(gamma[2]*iteration) };
double tw[3]={0.,0.,0.};
for (int dir=0; dir<2; dir++) {
Vertex* n = (dir==0)? v1 : v2;
while (n!=top){
uint i=n->id;
tw[0]+=1./M[i].values[0];
tw[1]+=1./M[i].values[1];
tw[2]+=1./M[i].values[2];
n=n->parent;
}
}
double beta[3] = {l*alpha[0]*d.values[0], l*alpha[1]*d.values[1], l*alpha[2]*d.values[2]};
beta[0]=(fabs(beta[0])>fabs(r.values[0]))?r.values[0]:beta[0];
beta[1]=(fabs(beta[1])>fabs(r.values[1]))?r.values[1]:beta[1];
beta[2]=(fabs(beta[2])>fabs(r.values[2]))?r.values[2]:beta[2];
DEBUG(2) << " alpha=" << alpha[0] << " " << alpha[1] << " " << alpha[2] << endl;
DEBUG(2) << " beta=" << beta[0] << " " << beta[1] << " " << beta[2] << endl;
for (int dir=0; dir<2; dir++) {
Vertex* n = (dir==0)? v1 : v2;
double sign=(dir==0)? -1. : 1.;
while (n!=top){
uint i=n->id;
assert(M[i].values[0]>0);
assert(M[i].values[1]>0);
assert(M[i].values[2]>0);
Pose delta( beta[0]/(M[i].values[0]*tw[0]), beta[1]/(M[i].values[1]*tw[1]), beta[2]/(M[i].values[2]*tw[2]));
delta=delta*sign;
DEBUG(2) << " " << dir << ":" << i <<"," << n->parent->id << ":"
<< n->parameters.x() << " " << n->parameters.y() << " " << n->parameters.theta() << " -> ";
n->parameters.x()+=delta.x();
n->parameters.y()+=delta.y();
n->parameters.theta()+=delta.theta();
DEBUG(2) << n->parameters.x() << " " << n->parameters.y() << " " << n->parameters.theta()<< endl;
n=n->parent;
}
}
updatePoseChain(v1,top);
updatePoseChain(v2,top);
Pose pf1=v1->pose;
Pose pf2=v2->pose;
DEBUG(2) << " pf1=" << pf1.x() << " " << pf1.y() << " " << pf1.theta() << endl;
DEBUG(2) << " pf2=" << pf2.x() << " " << pf2.y() << " " << pf2.theta() << endl;
DEBUG(2) << " en=" << p12.x()-pf2.x() << " " << p12.y()-pf2.y() << " " << p12.theta()-pf2.theta() << endl;
}
}
void TreeOptimizer2::iterate(TreePoseGraph2::EdgeSet* eset){
TreePoseGraph2::EdgeSet* temp=sortedEdges;
if (eset){
sortedEdges=eset;
}
computePreconditioner();
propagateErrors();
sortedEdges=temp;
}
void TreeOptimizer2::updatePoseChain(Vertex* v, Vertex* top){
if (v!=top){
updatePoseChain(v->parent, top);
v->pose.x()=v->parent->pose.x()+v->parameters.x();
v->pose.y()=v->parent->pose.y()+v->parameters.y();
v->pose.theta()=v->parent->pose.theta()+v->parameters.theta();
return;
}
}
TreeOptimizer2::Pose TreeOptimizer2::getPose(Vertex*v, Vertex* top){
Pose p(0,0,0);
Vertex* aux=v;
while (aux!=top){
p.x()+=aux->parameters.x();
p.y()+=aux->parameters.y();
p.theta()+=aux->parameters.theta();
aux=aux->parent;
}
p.x()+=aux->pose.x();
p.y()+=aux->pose.y();
p.theta()+=aux->pose.theta();
return p;
}
double TreeOptimizer2::error(const Edge* e) const{
const Vertex* v1=e->v1;
const Vertex* v2=e->v2;
Pose p1=v1->pose;
Pose p2=v2->pose;
DEBUG(2) << " p1=" << p1.x() << " " << p1.y() << " " << p1.theta() << endl;
DEBUG(2) << " p2=" << p2.x() << " " << p2.y() << " " << p2.theta() << endl;
Transformation et=e->transformation;
Transformation t1(p1);
Transformation t2(p2);
Transformation t12=t1*et;
Pose p12=t12.toPoseType();
DEBUG(2) << " pt2=" << p12.x() << " " << p12.y() << " " << p12.theta() << endl;
Pose r(p12.x()-p2.x(), p12.y()-p2.y(), p12.theta()-p2.theta());
double angle=r.theta();
angle=atan2(sin(angle),cos(angle));
r.theta()=angle;
DEBUG(2) << " e=" << r.x() << " " << r.y() << " " << r.theta() << endl;
InformationMatrix S=e->informationMatrix;
InformationMatrix R;
R.values[0][0]=t1.rotationMatrix[0][0];
R.values[0][1]=t1.rotationMatrix[0][1];
R.values[0][2]=0;
R.values[1][0]=t1.rotationMatrix[1][0];
R.values[1][1]=t1.rotationMatrix[1][1];
R.values[1][2]=0;
R.values[2][0]=0;
R.values[2][1]=0;
R.values[2][2]=1;
InformationMatrix W=R*S*R.transpose();
Pose r1=W*r;
return r.x()*r1.x()+r.y()*r1.y()+r.theta()*r1.theta();
}
double TreeOptimizer2::error() const{
double globalError=0.;
for (TreePoseGraph2::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
globalError+=error(it->second);
}
return globalError;
}
}; //namespace AISNavigation
corelib/src/toro3d/treeoptimizer2.hh
+107
View File
@@ -0,0 +1,107 @@
/**********************************************************************
*
* This source code is part of the Tree-based Network Optimizer (TORO)
*
* TORO Copyright (c) 2007 Giorgio Grisetti, Cyrill Stachniss,
* Slawomir Grzonka, and Wolfram Burgard
*
* TORO is licences under the Common Creative License,
* Attribution-NonCommercial-ShareAlike 3.0
*
* You are free:
* - to Share - to copy, distribute and transmit the work
* - to Remix - to adapt the work
*
* Under the following conditions:
*
* - Attribution. You must attribute the work in the manner specified
* by the author or licensor (but not in any way that suggests that
* they endorse you or your use of the work).
*
* - Noncommercial. You may not use this work for commercial purposes.
*
* - Share Alike. If you alter, transform, or build upon this work,
* you may distribute the resulting work only under the same or
* similar license to this one.
*
* Any of the above conditions can be waived if you get permission
* from the copyright holder. Nothing in this license impairs or
* restricts the author's moral rights.
*
* TORO is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
* PURPOSE.
**********************************************************************/
/** \file treeoptimizer2.hh
*
* \brief Defines the core optimizer class for 2D graphs which is a
* subclass of TreePoseGraph2
*
**/
#ifndef _TREEOPTIMIZER2_HH_
#define _TREEOPTIMIZER2_HH_
#include "posegraph2.hh"
namespace AISNavigation {
/** \brief Class that contains the core optimization algorithm **/
struct TreeOptimizer2: public TreePoseGraph2{
typedef std::vector<Pose> PoseVector;
/** Constructor **/
TreeOptimizer2();
/** Destructor **/
virtual ~TreeOptimizer2();
/** Initialization function **/
void initializeTreeParameters();
/** Initialization function **/
void initializeOptimization();
/** Initialization function **/
void initializeOnlineOptimization();
/** Performs one iteration of the algorithm **/
void iterate(TreePoseGraph2::EdgeSet* eset=0);
/** Conmputes the gloabl error of the network **/
double error() const;
protected:
/** The first of the two main steps of each iteration **/
void computePreconditioner();
/** The second of the two main steps of each iteration **/
void propagateErrors();
/** Recomputes the poses of all vertices from v to an arbitraty
parent (top) of v in the tree **/
void updatePoseChain(Vertex* v, Vertex* top);
/** Recomputes only the pose of the node v wrt. to an arbitraty
parent (top) of v in the tree **/
Pose getPose(Vertex*v, Vertex* top);
/** Conmputes the error of the constraint/edge e **/
double error(const Edge* e) const;
/** Iteration counter **/
int iteration;
/** Used to compute the learning rate lambda **/
double gamma[3];
/** The diaginal block elements of the preconditioning matrix (D_k
in the paper) **/
PoseVector M;
};
}; //namespace AISNavigation
#endif
corelib/src/util3d.cpp
+70 -59
View File
@@ -2118,7 +2118,7 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
float minX=-minMapSize/2.0, minY=-minMapSize/2.0, maxX=minMapSize/2.0, maxY=minMapSize/2.0;
bool undefinedSize = minMapSize == 0.0f;
float x,y,z,toll,pitch,yaw,cosT,sinT;
float x=0.0f,y=0.0f,z=0.0f,roll=0.0f,pitch=0.0f,yaw=0.0f,cosT=0.0f,sinT=0.0f;
cv::Mat affineTransform(2,3,CV_32FC1);
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
@@ -2127,7 +2127,7 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
UASSERT(!iter->second.isNull());
const std::pair<cv::Mat, cv::Mat> & pair = occupancy.at(iter->first);
iter->second.getTranslationAndEulerAngles(x,y,z,toll,pitch,yaw);
iter->second.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
cosT = cos(yaw);
sinT = sin(yaw);
affineTransform.at<float>(0,0) = cosT;
@@ -2210,76 +2210,87 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
yMin = minY-margin;
float xMax = maxX+margin;
float yMax = maxY+margin;
UDEBUG("map min=(%f, %f) max=(%f,%f)", xMin, yMin, xMax, yMax);
map = cv::Mat::ones((yMax - yMin) / cellSize + 0.5f, (xMax - xMin) / cellSize + 0.5f, CV_8S)*-1;
for(std::map<int, Transform>::const_iterator kter = poses.begin(); kter!=poses.end(); ++kter)
if(fabs((yMax - yMin) / cellSize) > 99999 ||
fabs((xMax - xMin) / cellSize) > 99999)
{
std::map<int, cv::Mat >::iterator iter = emptyLocalMaps.find(kter->first);
std::map<int, cv::Mat >::iterator jter = occupiedLocalMaps.find(kter->first);
if(iter!=emptyLocalMaps.end())
{
for(int i=0; i<iter->second.rows; ++i)
{
cv::Point2i pt((iter->second.at<float>(i,0)-xMin)/cellSize + 0.5f, (iter->second.at<float>(i,1)-yMin)/cellSize + 0.5f);
map.at<char>(pt.y, pt.x) = 0; // free space
}
}
if(jter!=occupiedLocalMaps.end())
{
for(int i=0; i<jter->second.rows; ++i)
{
cv::Point2i pt((jter->second.at<float>(i,0)-xMin)/cellSize + 0.5f, (jter->second.at<float>(i,1)-yMin)/cellSize + 0.5f);
map.at<char>(pt.y, pt.x) = 100; // obstacles
}
}
//UDEBUG("empty=%d occupied=%d", empty, occupied);
UERROR("Large map size!! map min=(%f, %f) max=(%f,%f). "
"There's maybe an error with the poses provided! The map will not be created!",
xMin, yMin, xMax, yMax);
}
// fill holes and remove empty from obstacle borders
cv::Mat updatedMap = map;
for(int i=2; i<map.rows-2; ++i)
else
{
for(int j=2; j<map.cols-2; ++j)
UDEBUG("map min=(%f, %f) max=(%f,%f)", xMin, yMin, xMax, yMax);
map = cv::Mat::ones((yMax - yMin) / cellSize + 0.5f, (xMax - xMin) / cellSize + 0.5f, CV_8S)*-1;
for(std::map<int, Transform>::const_iterator kter = poses.begin(); kter!=poses.end(); ++kter)
{
if(map.at<char>(i, j) == -1 &&
map.at<char>(i+1, j) != -1 &&
map.at<char>(i-1, j) != -1 &&
map.at<char>(i, j+1) != -1 &&
map.at<char>(i, j-1) != -1)
std::map<int, cv::Mat >::iterator iter = emptyLocalMaps.find(kter->first);
std::map<int, cv::Mat >::iterator jter = occupiedLocalMaps.find(kter->first);
if(iter!=emptyLocalMaps.end())
{
updatedMap.at<char>(i, j) = 0;
for(int i=0; i<iter->second.rows; ++i)
{
cv::Point2i pt((iter->second.at<float>(i,0)-xMin)/cellSize + 0.5f, (iter->second.at<float>(i,1)-yMin)/cellSize + 0.5f);
map.at<char>(pt.y, pt.x) = 0; // free space
}
}
else if(map.at<char>(i, j) == 100)
if(jter!=occupiedLocalMaps.end())
{
// obstacle/empty/unknown -> remove empty
// unknown/empty/obstacle -> remove empty
if(map.at<char>(i-1, j) == 0 &&
map.at<char>(i-2, j) == -1)
for(int i=0; i<jter->second.rows; ++i)
{
updatedMap.at<char>(i-1, j) = -1;
}
else if(map.at<char>(i+1, j) == 0 &&
map.at<char>(i+2, j) == -1)
{
updatedMap.at<char>(i+1, j) = -1;
}
if(map.at<char>(i, j-1) == 0 &&
map.at<char>(i, j-2) == -1)
{
updatedMap.at<char>(i, j-1) = -1;
}
else if(map.at<char>(i, j+1) == 0 &&
map.at<char>(i, j+2) == -1)
{
updatedMap.at<char>(i, j+1) = -1;
cv::Point2i pt((jter->second.at<float>(i,0)-xMin)/cellSize + 0.5f, (jter->second.at<float>(i,1)-yMin)/cellSize + 0.5f);
map.at<char>(pt.y, pt.x) = 100; // obstacles
}
}
//UDEBUG("empty=%d occupied=%d", empty, occupied);
}
// fill holes and remove empty from obstacle borders
cv::Mat updatedMap = map;
for(int i=2; i<map.rows-2; ++i)
{
for(int j=2; j<map.cols-2; ++j)
{
if(map.at<char>(i, j) == -1 &&
map.at<char>(i+1, j) != -1 &&
map.at<char>(i-1, j) != -1 &&
map.at<char>(i, j+1) != -1 &&
map.at<char>(i, j-1) != -1)
{
updatedMap.at<char>(i, j) = 0;
}
else if(map.at<char>(i, j) == 100)
{
// obstacle/empty/unknown -> remove empty
// unknown/empty/obstacle -> remove empty
if(map.at<char>(i-1, j) == 0 &&
map.at<char>(i-2, j) == -1)
{
updatedMap.at<char>(i-1, j) = -1;
}
else if(map.at<char>(i+1, j) == 0 &&
map.at<char>(i+2, j) == -1)
{
updatedMap.at<char>(i+1, j) = -1;
}
if(map.at<char>(i, j-1) == 0 &&
map.at<char>(i, j-2) == -1)
{
updatedMap.at<char>(i, j-1) = -1;
}
else if(map.at<char>(i, j+1) == 0 &&
map.at<char>(i, j+2) == -1)
{
updatedMap.at<char>(i, j+1) = -1;
}
}
}
}
map = updatedMap;
}
map = updatedMap;
}
UDEBUG("timer=%fs", timer.ticks());
return map;
guilib/src/DatabaseViewer.cpp
+110 -39
View File
@@ -89,6 +89,9 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
ui_->dockWidget_visual->setVisible(false);
ui_->dockWidget_stereoView->setVisible(false);
ui_->dockWidget_view3d->setVisible(false);
ui_->dockWidget_detectMoreLC->setVisible(false);
ui_->dockWidget_graphOptimizer->setVisible(false);
ui_->dockWidget_gridcloud->setVisible(false);
ui_->constraintsViewer->setCameraLockZ(false);
ui_->constraintsViewer->setCameraFree();
@@ -101,6 +104,9 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
ui_->menuView->addAction(ui_->dockWidget_visual->toggleViewAction());
ui_->menuView->addAction(ui_->dockWidget_stereoView->toggleViewAction());
ui_->menuView->addAction(ui_->dockWidget_view3d->toggleViewAction());
ui_->menuView->addAction(ui_->dockWidget_detectMoreLC->toggleViewAction());
ui_->menuView->addAction(ui_->dockWidget_graphOptimizer->toggleViewAction());
ui_->menuView->addAction(ui_->dockWidget_gridcloud->toggleViewAction());
connect(ui_->dockWidget_graphView->toggleViewAction(), SIGNAL(triggered()), this, SLOT(updateGraphView()));
connect(ui_->actionQuit, SIGNAL(triggered()), this, SLOT(close()));
@@ -162,26 +168,33 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
connect(ui_->horizontalSlider_iterations, SIGNAL(valueChanged(int)), this, SLOT(sliderIterationsValueChanged(int)));
connect(ui_->horizontalSlider_iterations, SIGNAL(sliderMoved(int)), this, SLOT(sliderIterationsValueChanged(int)));
connect(ui_->spinBox_iterations, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
connect(ui_->spinBox_optimizationsFrom, SIGNAL(valueChanged(int)), this, SLOT(updateGraphView()));
connect(ui_->checkBox_initGuess, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
connect(ui_->spinBox_optimizationsFrom, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
connect(ui_->checkBox_ignoreCovariance, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
connect(ui_->checkBox_gridFromProjection, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
connect(ui_->comboBox_graphOptimizer, SIGNAL(currentIndexChanged(int)), this, SLOT(updateGraphView()));
connect(ui_->checkBox_2dslam, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
connect(ui_->groupBox_gridFromProjection, SIGNAL(clicked(bool)), this, SLOT(updateGrid()));
connect(ui_->doubleSpinBox_gridCellSize, SIGNAL(editingFinished()), this, SLOT(updateGrid()));
connect(ui_->spinBox_projDecimation, SIGNAL(editingFinished()), this, SLOT(updateGrid()));
connect(ui_->doubleSpinBox_projMaxDepth, SIGNAL(editingFinished()), this, SLOT(updateGrid()));
// connect configuration changed
connect(ui_->graphViewer, SIGNAL(configChanged()), this, SLOT(configModified()));
//connect(ui_->graphicsView_A, SIGNAL(configChanged()), this, SLOT(configModified()));
//connect(ui_->graphicsView_B, SIGNAL(configChanged()), this, SLOT(configModified()));
// Graph view
connect(ui_->spinBox_iterations, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
connect(ui_->checkBox_initGuess, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
connect(ui_->checkBox_ignoreCovariance, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
connect(ui_->checkBox_gridFromProjection, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
connect(ui_->comboBox_graphOptimizer, SIGNAL(currentIndexChanged(int)), this, SLOT(configModified()));
connect(ui_->checkBox_2dslam, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
connect(ui_->groupBox_gridFromProjection, SIGNAL(clicked(bool)), this, SLOT(configModified()));
connect(ui_->doubleSpinBox_gridCellSize, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
connect(ui_->spinBox_projDecimation, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
connect(ui_->doubleSpinBox_projMaxDepth, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
connect(ui_->groupBox_posefiltering, SIGNAL(clicked(bool)), this, SLOT(configModified()));
connect(ui_->doubleSpinBox_posefilteringRadius, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
connect(ui_->doubleSpinBox_posefilteringAngle, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
// ICP parameters
connect(ui_->spinBox_icp_decimation, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
connect(ui_->doubleSpinBox_icp_maxDepth, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
@@ -192,9 +205,10 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
connect(ui_->spinBox_icp_normalKSearch, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
connect(ui_->checkBox_icp_2d, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
// Visual parameters
connect(ui_->checkBox_visual_recomputeFeatures, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
connect(ui_->groupBox_visual_recomputeFeatures, SIGNAL(clicked(bool)), this, SLOT(configModified()));
connect(ui_->comboBox_featureType, SIGNAL(currentIndexChanged(int)), this, SLOT(configModified()));
connect(ui_->comboBox_nnType, SIGNAL(currentIndexChanged(int)), this, SLOT(configModified()));
connect(ui_->checkBox_visual_2d, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
connect(ui_->doubleSpinBox_visual_hessian, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
connect(ui_->doubleSpinBox_visual_nndr, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
connect(ui_->spinBox_visual_minCorrespondences, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
connect(ui_->doubleSpinBox_visual_maxCorrespDistance, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
@@ -216,6 +230,9 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
ui_->dockWidget_stereoView->installEventFilter(this);
ui_->dockWidget_visual->installEventFilter(this);
ui_->dockWidget_view3d->installEventFilter(this);
ui_->dockWidget_detectMoreLC->installEventFilter(this);
ui_->dockWidget_graphOptimizer->installEventFilter(this);
ui_->dockWidget_gridcloud->installEventFilter(this);
}
DatabaseViewer::~DatabaseViewer()
@@ -263,15 +280,23 @@ void DatabaseViewer::readSettings()
savedMaximized_ = settings.value("maximized", false).toBool();
// GraphViewer settings
settings.beginGroup("GraphView");
ui_->graphViewer->loadSettings(settings);
ui_->graphViewer->loadSettings(settings, "GraphView");
settings.beginGroup("optimization");
ui_->spinBox_iterations->setValue(settings.value("iterations", ui_->spinBox_iterations->value()).toInt());
ui_->checkBox_initGuess->setChecked(settings.value("initGuess", ui_->checkBox_initGuess->isChecked()).toBool());
ui_->checkBox_ignoreCovariance->setChecked(settings.value("ignoreCovariance", ui_->checkBox_ignoreCovariance->isChecked()).toBool());
ui_->checkBox_gridFromProjection->setChecked(settings.value("gridFromProj", ui_->checkBox_gridFromProjection->isChecked()).toBool());
ui_->comboBox_graphOptimizer->setCurrentIndex(settings.value("strategy", ui_->comboBox_graphOptimizer->currentIndex()).toInt());
ui_->checkBox_2dslam->setChecked(settings.value("slam2d", ui_->checkBox_2dslam->isChecked()).toBool());
settings.endGroup();
settings.beginGroup("grid");
ui_->groupBox_gridFromProjection->setChecked(settings.value("gridFromProj", ui_->groupBox_gridFromProjection->isChecked()).toBool());
ui_->doubleSpinBox_gridCellSize->setValue(settings.value("gridCellSize", ui_->doubleSpinBox_gridCellSize->value()).toDouble());
ui_->spinBox_projDecimation->setValue(settings.value("projDecimation", ui_->spinBox_projDecimation->value()).toInt());
ui_->doubleSpinBox_projMaxDepth->setValue(settings.value("projMaxDepth", ui_->doubleSpinBox_projMaxDepth->value()).toDouble());
ui_->groupBox_posefiltering->setChecked(settings.value("poseFiltering", ui_->groupBox_posefiltering->isChecked()).toBool());
ui_->doubleSpinBox_posefilteringRadius->setValue(settings.value("poseFilteringRadius", ui_->doubleSpinBox_posefilteringRadius->value()).toDouble());
ui_->doubleSpinBox_posefilteringAngle->setValue(settings.value("poseFilteringAngle", ui_->doubleSpinBox_posefilteringAngle->value()).toDouble());
settings.endGroup();
// ImageViews
@@ -292,9 +317,10 @@ void DatabaseViewer::readSettings()
// Visual parameters
settings.beginGroup("visual");
ui_->checkBox_visual_recomputeFeatures->setChecked(settings.value("reextract", ui_->checkBox_visual_recomputeFeatures->isChecked()).toBool());
ui_->groupBox_visual_recomputeFeatures->setChecked(settings.value("reextract", ui_->groupBox_visual_recomputeFeatures->isChecked()).toBool());
ui_->comboBox_featureType->setCurrentIndex(settings.value("featureType", ui_->comboBox_featureType->currentIndex()).toInt());
ui_->comboBox_nnType->setCurrentIndex(settings.value("nnType", ui_->comboBox_nnType->currentIndex()).toInt());
ui_->checkBox_visual_2d->setChecked(settings.value("force2d", ui_->checkBox_visual_2d->isChecked()).toBool());
ui_->doubleSpinBox_visual_hessian->setValue(settings.value("hessian", ui_->doubleSpinBox_visual_hessian->value()).toDouble());
ui_->doubleSpinBox_visual_nndr->setValue(settings.value("nndr", ui_->doubleSpinBox_visual_nndr->value()).toDouble());
ui_->spinBox_visual_minCorrespondences->setValue(settings.value("minCorr", ui_->spinBox_visual_minCorrespondences->value()).toInt());
ui_->doubleSpinBox_visual_maxCorrespDistance->setValue(settings.value("maxCorrDist", ui_->doubleSpinBox_visual_maxCorrespDistance->value()).toDouble());
@@ -324,15 +350,25 @@ void DatabaseViewer::writeSettings()
savedMaximized_ = this->isMaximized();
// save GraphViewer settings
settings.beginGroup("GraphView");
ui_->graphViewer->saveSettings(settings);
ui_->graphViewer->saveSettings(settings, "GraphView");
// save optimization settings
settings.beginGroup("optimization");
settings.setValue("iterations", ui_->spinBox_iterations->value());
settings.setValue("initGuess", ui_->checkBox_initGuess->isChecked());
settings.setValue("ignoreCovariance", ui_->checkBox_ignoreCovariance->isChecked());
settings.setValue("gridFromProj", ui_->checkBox_gridFromProjection->isChecked());
settings.setValue("strategy", ui_->comboBox_graphOptimizer->currentIndex());
settings.setValue("slam2d", ui_->checkBox_2dslam->isChecked());
settings.endGroup();
// save Grid settings
settings.beginGroup("grid");
settings.setValue("gridFromProj", ui_->groupBox_gridFromProjection->isChecked());
settings.setValue("gridCellSize", ui_->doubleSpinBox_gridCellSize->value());
settings.setValue("projDecimation", ui_->spinBox_projDecimation->value());
settings.setValue("projMaxDepth", ui_->doubleSpinBox_projMaxDepth->value());
settings.setValue("poseFiltering", ui_->groupBox_posefiltering->isChecked());
settings.setValue("poseFilteringRadius", ui_->doubleSpinBox_posefilteringRadius->value());
settings.setValue("poseFilteringAngle", ui_->doubleSpinBox_posefilteringAngle->value());
settings.endGroup();
// ImageViews
@@ -353,9 +389,10 @@ void DatabaseViewer::writeSettings()
// save Visual parameters
settings.beginGroup("visual");
settings.setValue("reextract", ui_->checkBox_visual_recomputeFeatures->isChecked());
settings.setValue("reextract", ui_->groupBox_visual_recomputeFeatures->isChecked());
settings.setValue("featureType", ui_->comboBox_featureType->currentIndex());
settings.setValue("nnType", ui_->comboBox_nnType->currentIndex());
settings.setValue("force2d", ui_->checkBox_visual_2d->isChecked());
settings.setValue("hessian", ui_->doubleSpinBox_visual_hessian->value());
settings.setValue("nndr", ui_->doubleSpinBox_visual_nndr->value());
settings.setValue("minCorr", ui_->spinBox_visual_minCorrespondences->value());
settings.setValue("maxCorrDist", ui_->doubleSpinBox_visual_maxCorrespDistance->value());
@@ -678,6 +715,7 @@ void DatabaseViewer::updateIds()
linksAdded_.clear();
linksRefined_.clear();
linksRemoved_.clear();
ui_->label_optimizeFrom->setText(tr("Optimize from"));
if(memory_->getLastWorkingSignature())
{
//get constraints only for parent links
@@ -710,7 +748,8 @@ void DatabaseViewer::updateIds()
int first = *ids.begin();
ui_->spinBox_optimizationsFrom->setRange(first, memory_->getLastWorkingSignature()->id());
ui_->spinBox_optimizationsFrom->setValue(first);
ui_->spinBox_optimizationsFrom->setValue(memory_->getLastWorkingSignature()->id());
ui_->label_optimizeFrom->setText(tr("Optimize from [%1, %2]").arg(first).arg(memory_->getLastWorkingSignature()->id()));
}
}
@@ -851,7 +890,7 @@ void DatabaseViewer::generateTOROGraph()
QString path = QFileDialog::getSaveFileName(this, tr("Save File"), pathDatabase_+"/constraints" + QString::number(id) + ".graph", tr("TORO file (*.graph)"));
if(!path.isEmpty())
{
rtabmap::graph::saveTOROGraph(path.toStdString(), uValueAt(graphes_, id), links);
graph::TOROOptimizer::saveGraph(path.toStdString(), uValueAt(graphes_, id), links);
}
}
}
@@ -2080,7 +2119,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
{
UTimer time;
bool added = false;
if(ui_->checkBox_gridFromProjection->isChecked())
if(ui_->groupBox_gridFromProjection->isChecked())
{
Signature data = memory_->getSignatureData(ids_.at(i), true);
if(!data.getDepthRaw().empty())
@@ -2159,13 +2198,28 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
std::map<int, rtabmap::Transform> & graph = uValueAt(graphes_, value);
std::multimap<int, Link> links = updateLinksWithModifications(links_);
ui_->graphViewer->updateGraph(graph, links);
if(graph.size() && localMaps_.size())
if(graph.size() && localMaps_.size() && ui_->graphViewer->isGridMapVisible())
{
float xMin, yMin;
float cell = ui_->doubleSpinBox_gridCellSize->value();
cv::Mat map = rtabmap::util3d::convertMap2Image8U(rtabmap::util3d::create2DMapFromOccupancyLocalMaps(graph, localMaps_, cell, xMin, yMin));
ui_->graphViewer->updateMap(map, cell, xMin, yMin);
cv::Mat map;
if(ui_->groupBox_posefiltering->isChecked())
{
std::map<int, rtabmap::Transform> graphFiltered = graph::radiusPosesFiltering(graph,
ui_->doubleSpinBox_posefilteringRadius->value(),
ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
map = rtabmap::util3d::create2DMapFromOccupancyLocalMaps(graphFiltered, localMaps_, cell, xMin, yMin);
}
else
{
map = rtabmap::util3d::create2DMapFromOccupancyLocalMaps(graph, localMaps_, cell, xMin, yMin);
}
if(!map.empty())
{
ui_->graphViewer->updateMap(rtabmap::util3d::convertMap2Image8U(map), cell, xMin, yMin);
}
}
ui_->graphViewer->update();
ui_->label_iterations->setNum(value);
//compute total length (neighbor links)
@@ -2209,16 +2263,31 @@ void DatabaseViewer::updateGraphView()
graphes_.push_back(poses_);
ui_->actionGenerate_TORO_graph_graph->setEnabled(true);
std::multimap<int, rtabmap::Link> links = updateLinksWithModifications(links_);
std::map<int, int> depthGraph = rtabmap::graph::generateDepthGraph(links, ui_->spinBox_optimizationsFrom->value(), 0);
rtabmap::graph::optimizeTOROGraph(
depthGraph,
graph::Optimizer * optimizer = 0;
if(ui_->comboBox_graphOptimizer->currentIndex() == graph::Optimizer::kTypeG2O)
{
optimizer = new graph::G2OOptimizer(ui_->spinBox_iterations->value(), ui_->checkBox_2dslam->isChecked(), ui_->checkBox_ignoreCovariance->isChecked());
}
else
{
optimizer = new graph::TOROOptimizer(ui_->spinBox_iterations->value(), ui_->checkBox_2dslam->isChecked(), ui_->checkBox_ignoreCovariance->isChecked());
}
int fromId = ui_->spinBox_optimizationsFrom->value();
std::map<int, rtabmap::Transform> posesOut;
std::multimap<int, rtabmap::Link> linksOut;
optimizer->getConnectedGraph(
fromId,
poses_,
links, finalPoses,
ui_->spinBox_iterations->value(),
ui_->checkBox_initGuess->isChecked(),
ui_->checkBox_ignoreCovariance->isChecked(),
&graphes_);
links,
posesOut,
linksOut);
QTime time;
time.start();
finalPoses = optimizer->optimize(fromId, posesOut, linksOut, &graphes_);
ui_->label_timeOptimization->setNum(double(time.elapsed())/1000.0);
graphes_.push_back(finalPoses);
delete optimizer;
}
if(graphes_.size())
{
@@ -2238,8 +2307,8 @@ void DatabaseViewer::updateGraphView()
void DatabaseViewer::updateGrid()
{
if((sender() != ui_->spinBox_projDecimation && sender() != ui_->doubleSpinBox_projMaxDepth) ||
(sender() == ui_->spinBox_projDecimation && ui_->checkBox_gridFromProjection->isChecked()) ||
(sender() == ui_->doubleSpinBox_projMaxDepth && ui_->checkBox_gridFromProjection->isChecked()))
(sender() == ui_->spinBox_projDecimation && ui_->groupBox_gridFromProjection->isChecked()) ||
(sender() == ui_->doubleSpinBox_projMaxDepth && ui_->groupBox_gridFromProjection->isChecked()))
{
localMaps_.clear();
updateGraphView();
@@ -2535,11 +2604,12 @@ void DatabaseViewer::refineConstraintVisually(int from, int to, bool updateGraph
std::string rejectedMsg;
double variance = -1.0;
int inliers = -1;
if(ui_->checkBox_visual_recomputeFeatures->isChecked())
if(ui_->groupBox_visual_recomputeFeatures->isChecked())
{
// create a fake memory to regenerate features
ParametersMap parameters;
parameters.insert(ParametersPair(Parameters::kSURFHessianThreshold(), uNumber2Str(ui_->doubleSpinBox_visual_hessian->value())));
parameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(ui_->comboBox_featureType->currentIndex())));
parameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(ui_->comboBox_nnType->currentIndex())));
parameters.insert(ParametersPair(Parameters::kLccBowInlierDistance(), uNumber2Str(ui_->doubleSpinBox_visual_maxCorrespDistance->value())));
parameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(ui_->doubleSpinBox_visual_maxDepth->value())));
parameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(ui_->doubleSpinBox_visual_nndr->value())));
@@ -2645,11 +2715,12 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
std::string rejectedMsg;
double variance = -1.0;
int inliers = -1;
if(ui_->checkBox_visual_recomputeFeatures->isChecked())
if(ui_->groupBox_visual_recomputeFeatures->isChecked())
{
// create a fake memory to regenerate features
ParametersMap parameters;
parameters.insert(ParametersPair(Parameters::kSURFHessianThreshold(), uNumber2Str(ui_->doubleSpinBox_visual_hessian->value())));
parameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(ui_->comboBox_featureType->currentIndex())));
parameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(ui_->comboBox_nnType->currentIndex())));
parameters.insert(ParametersPair(Parameters::kLccBowInlierDistance(), uNumber2Str(ui_->doubleSpinBox_visual_maxCorrespDistance->value())));
parameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(ui_->doubleSpinBox_visual_maxDepth->value())));
parameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(ui_->doubleSpinBox_visual_nndr->value())));
guilib/src/MainWindow.cpp
+26 -10
View File
@@ -3145,12 +3145,9 @@ void MainWindow::postProcessing()
_initProgressDialog->show();
ParametersMap parameters = _preferencesDialog->getAllParameters();
int toroIterations = Parameters::defaultRGBDToroIterations();
bool ignoreVariance = Parameters::defaultRGBDToroIgnoreVariance();
bool toroOptimizeFromGraphEnd = Parameters::defaultRGBDOptimizeFromGraphEnd();
Parameters::parse(parameters, Parameters::kRGBDToroIterations(), toroIterations);
Parameters::parse(parameters, Parameters::kRGBDToroIgnoreVariance(), ignoreVariance);
Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), toroOptimizeFromGraphEnd);
graph::Optimizer * optimizer = graph::Optimizer::create(parameters);
bool optimizeFromGraphEnd = Parameters::defaultRGBDOptimizeFromGraphEnd();
Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), optimizeFromGraphEnd);
int loopClosuresAdded = 0;
if(detectMoreLoopClosures)
@@ -3282,9 +3279,17 @@ void MainWindow::postProcessing()
{
_initProgressDialog->appendText(tr("Optimizing graph with new links (%1 nodes, %2 constraints)...")
.arg(odomPoses.size()).arg(_currentLinksMap.size()));
int fromId = optimizeFromGraphEnd?odomPoses.rbegin()->first:odomPoses.begin()->first;
std::map<int, rtabmap::Transform> posesOut;
std::multimap<int, rtabmap::Link> linksOut;
std::map<int, rtabmap::Transform> optimizedPoses;
std::map<int, int> depthGraph = rtabmap::graph::generateDepthGraph(_currentLinksMap, toroOptimizeFromGraphEnd?odomPoses.rbegin()->first:odomPoses.begin()->first);
rtabmap::graph::optimizeTOROGraph(depthGraph, odomPoses, _currentLinksMap, optimizedPoses, toroIterations, true, ignoreVariance);
optimizer->getConnectedGraph(
fromId,
odomPoses,
_currentLinksMap,
posesOut,
linksOut);
optimizedPoses = optimizer->optimize(fromId, posesOut, linksOut);
_currentPosesMap = optimizedPoses;
_initProgressDialog->appendText(tr("Optimizing graph with new links... done!"));
}
@@ -3437,9 +3442,18 @@ void MainWindow::postProcessing()
_initProgressDialog->appendText(tr("Optimizing graph with updated links (%1 nodes, %2 constraints)...")
.arg(odomPoses.size()).arg(_currentLinksMap.size()));
int fromId = optimizeFromGraphEnd?odomPoses.rbegin()->first:odomPoses.begin()->first;
std::map<int, rtabmap::Transform> posesOut;
std::multimap<int, rtabmap::Link> linksOut;
std::map<int, rtabmap::Transform> optimizedPoses;
std::map<int, int> depthGraph = rtabmap::graph::generateDepthGraph(_currentLinksMap, toroOptimizeFromGraphEnd?odomPoses.rbegin()->first:odomPoses.begin()->first);
rtabmap::graph::optimizeTOROGraph(depthGraph, odomPoses, _currentLinksMap, optimizedPoses, toroIterations, true, ignoreVariance);
optimizer->getConnectedGraph(
fromId,
odomPoses,
_currentLinksMap,
posesOut,
linksOut);
optimizedPoses = optimizer->optimize(fromId, posesOut, linksOut);
_initProgressDialog->appendText(tr("Optimizing graph with updated links... done!"));
_initProgressDialog->incrementStep();
@@ -3449,6 +3463,8 @@ void MainWindow::postProcessing()
_initProgressDialog->setValue(_initProgressDialog->maximumSteps());
_initProgressDialog->appendText("Post-processing finished!");
delete optimizer;
}
void MainWindow::deleteMemory()
guilib/src/PreferencesDialog.cpp
+41 -12
View File
@@ -53,6 +53,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/Camera.h"
#include "rtabmap/core/Memory.h"
#include "rtabmap/core/VWDictionary.h"
#include "rtabmap/core/Graph.h"
#include "rtabmap/gui/LoopClosureViewer.h"
#include "rtabmap/gui/DataRecorder.h"
@@ -119,11 +120,11 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
#endif
#ifdef _WIN32
_ui->radioButton_openni2->setChecked(true);
_ui->radioButton_opennipcl->setChecked(false);
_ui->groupBox_openni2->setVisible(true);
_ui->radioButton_openni2->setChecked(true);
_ui->radioButton_opennipcl->setChecked(false);
_ui->groupBox_openni2->setVisible(true);
#else
_ui->groupBox_openni2->setVisible(false);
_ui->groupBox_openni2->setVisible(false);
#endif
if(RTABMAP_NONFREE == 0)
@@ -139,6 +140,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->reextract_nn->setItemData(1, 0, Qt::UserRole - 1);
_ui->odom_bin_nn->setItemData(1, 0, Qt::UserRole - 1);
}
if(!graph::G2OOptimizer::available())
{
_ui->graphOptimization_type->setItemData(1, 0, Qt::UserRole - 1);
}
_ui->predictionPlot->showLegend(false);
@@ -455,11 +460,14 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->rgdb_angularUpdate->setObjectName(Parameters::kRGBDAngularUpdate().c_str());
_ui->rgdb_newMapOdomChange->setObjectName(Parameters::kRGBDNewMapOdomChangeDistance().c_str());
_ui->odomScanHistory->setObjectName(Parameters::kRGBDPoseScanMatching().c_str());
_ui->globalDetection_toroIterations->setObjectName(Parameters::kRGBDToroIterations().c_str());
_ui->globalDetection_toroIgnoreVariance->setObjectName(Parameters::kRGBDToroIgnoreVariance().c_str());
_ui->globalDetection_optimizeFromGraphEnd->setObjectName(Parameters::kRGBDOptimizeFromGraphEnd().c_str());
_ui->spinBox_maxLocalLocationsRetrieved->setObjectName(Parameters::kRGBDMaxLocalRetrieved().c_str());
_ui->graphOptimization_type->setObjectName(Parameters::kRGBDOptimizeStrategy().c_str());
_ui->graphOptimization_slam2d->setObjectName(Parameters::kRGBDOptimizeSlam2D().c_str());
_ui->graphOptimization_iterations->setObjectName(Parameters::kRGBDOptimizeIterations().c_str());
_ui->graphOptimization_covarianceIgnored->setObjectName(Parameters::kRGBDOptimizeVarianceIgnored().c_str());
_ui->graphOptimization_fromGraphEnd->setObjectName(Parameters::kRGBDOptimizeFromGraphEnd().c_str());
_ui->graphPlan_goalReachedRadius->setObjectName(Parameters::kRGBDGoalReachedRadius().c_str());
_ui->graphPlan_planWithNearNodesLinked->setObjectName(Parameters::kRGBDPlanWithNearNodesLinked().c_str());
@@ -1544,6 +1552,18 @@ bool PreferencesDialog::validateForm()
}
}
// optimization strategy
if(!graph::G2OOptimizer::available())
{
if(_ui->graphOptimization_type->currentIndex() > 0)
{
QMessageBox::warning(this, tr("Parameter warning"),
tr("Selected graph optimization strategy (g2o) is not available. RTAB-Map is not built "
"with g2o. TORO is set instead for graph optimization strategy."));
_ui->graphOptimization_type->setCurrentIndex(graph::Optimizer::kTypeTORO);
}
}
//verify binary features and nearest neighbor
// BOW dictionary type
if(_ui->comboBox_dictionary_strategy->currentIndex() == VWDictionary::kNNFlannLSH && _ui->comboBox_detector_strategy->currentIndex() <= 1)
@@ -2120,6 +2140,7 @@ void PreferencesDialog::setParameter(const std::string & key, const std::string
"with the nonfree module from OpenCV. Keeping default combo value: %s.",
combo->objectName().toStdString().c_str(),
combo->currentText().toStdString().c_str());
ok = false;
}
else if(valueInt==1 &&
(combo->objectName().toStdString().compare(Parameters::kKpNNStrategy()) == 0 ||
@@ -2132,13 +2153,21 @@ void PreferencesDialog::setParameter(const std::string & key, const std::string
"with binary descriptors. Keeping default combo value: %s.",
combo->objectName().toStdString().c_str(),
combo->currentText().toStdString().c_str());
}
else
{
combo->setCurrentIndex(valueInt);
ok = false;
}
}
else
if(!graph::G2OOptimizer::available())
{
if(valueInt==1 && combo->objectName().toStdString().compare(Parameters::kRGBDOptimizeStrategy()) == 0)
{
UWARN("Trying to set \"%s\" to g2o but RTAB-Map isn't built "
"with g2o. Keeping default combo value: %s.",
combo->objectName().toStdString().c_str(),
combo->currentText().toStdString().c_str());
ok = false;
}
}
if(ok)
{
combo->setCurrentIndex(valueInt);
}
guilib/src/ui/DatabaseViewer.ui
+583 -343
View File
File diff suppressed because it is too large Load Diff
guilib/src/ui/preferencesDialog.ui
+99 -37
View File
@@ -63,9 +63,9 @@
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<y>-171</y>
<width>744</width>
<height>1110</height>
<height>1389</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>7</number>
<number>19</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29">
@@ -5017,7 +5017,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<item row="3" column="1">
<widget class="QLabel" name="label_scanMatching">
<property name="text">
<string>Odometry pose correction using laser scan matching. ICP 2D only is used here.</string>
<string>Pose scan matching: odometry pose correction using laser scan matching. ICP 2D only is used here.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -5067,7 +5067,28 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
<layout class="QGridLayout" name="gridLayout_49" columnstretch="0,1">
<item row="0" column="0">
<widget class="QSpinBox" name="globalDetection_toroIterations">
<widget class="QComboBox" name="graphOptimization_type">
<item>
<property name="text">
<string>TORO</string>
</property>
</item>
<item>
<property name="text">
<string>g2o</string>
</property>
</item>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_31">
<property name="text">
<string>Iterations.</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QSpinBox" name="graphOptimization_iterations">
<property name="minimum">
<number>0</number>
</property>
@@ -5079,44 +5100,85 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_31">
<property name="text">
<string>TORO graph optimization iterations</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QCheckBox" name="globalDetection_toroIgnoreVariance">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_141">
<property name="text">
<string>Ignore constraints' variance. If checked, identity information matrix is used for each constraint in TORO. Otherwise, an information matrix is generated from the variance saved in the links.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QCheckBox" name="globalDetection_optimizeFromGraphEnd">
<widget class="QCheckBox" name="graphOptimization_covarianceIgnored">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_141">
<property name="text">
<string>Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links (transitional and rotational variances).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QCheckBox" name="graphOptimization_fromGraphEnd">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_151">
<property name="text">
<string>Optimize graph from the newest node.
-If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). An odometry correction from frames /map to /odom is computed.
-If true, there is no odometry correction computed since it is all previous poses in the map that are corrected, not the last one (which corresponds to latest odometry value).
Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).</string>
<string>Optimize graph from the newest node.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="Line" name="line">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QCheckBox" name="graphOptimization_slam2d">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_164">
<property name="text">
<string>2d SLAM: use fast 3DoF (x, y, theta) optimization instead of 6DoF (x, y, z, roll, pitch, yaw) optimization.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_45">
<property name="text">
<string>Graph optimization algorithm.</string>
</property>
</widget>
</item>
<item row="7" column="1">
<widget class="QLabel" name="label_183">
<property name="text">
<string>-If false, the graph is optimized from the oldest node of the current graph. It can be useful to preserve the map referential from the oldest node. An odometry correction between frames /map to /odom is computed. Warning: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_211">
<property name="text">
<string>-If true, there is no odometry correction computed. All previous poses in the map are corrected instead, not the last one (which corresponds to latest odometry value). So, the transform between frames /map to /odom will be always Identity even on loop closures.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -5138,7 +5200,7 @@ Warning when set to false: when some nodes are transferred, the first referentia
<item>
<widget class="QLabel" name="label_54">
<property name="text">
<string>Activate local detection over all locations in STM. The Bayes filter is not used here: If there are enough correspondences between the current image and others in STM, transformations are computed. The same 2-steps technique as for global loop closure constraints is used here. This generates more constraints in the map's graph, so more time is required to optimize the graph.</string>
<string>Activate local detection over all locations in STM. The Bayes filter is not used here: If there are enough correspondences between the current image and others in STM, transformations are computed. This generates more constraints in the map's graph, so more time is required to optimize the graph.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -5160,7 +5222,7 @@ Warning when set to false: when some nodes are transferred, the first referentia
<item>
<widget class="QLabel" name="label_space1_2">
<property name="text">
<string>Activate local detection over locations (in Working Memory) near in space. The Bayes filter is not used here, so it may results in more false detections. ICP 2D only is used here. Only nodes in the local radius are used.</string>
<string>Activate local detection over locations (in Working Memory) near in space. The Bayes filter is not used here, so it may results in more false detections. ICP 2D only is used here. Only nodes in the local radius are used. The laser scans are compared to those inside a radius around the current estimated position of the robot. If the laser scans match, a constraint is added to the graph. Useful when the robot is visiting backward (camera not in the same direction) already visited locations.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -5727,7 +5789,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
<item>
<widget class="QGroupBox" name="groupBox_loopClosure_icp3">
<property name="title">
<string>ICP 3D - Point To Plane (e.g. Kinect depth)</string>
<string>ICP 3D (e.g. Kinect depth)</string>
</property>
<layout class="QGridLayout" name="gridLayout_53" columnstretch="0,1">
<item row="0" column="0">
package.xml
+1 -1
View File
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package>
<name>rtabmap</name>
<version>0.8.6</version>
<version>0.8.7</version>
<description>RTAB-Map's standalone library. RTAB-Map is an RGB-D SLAM approach with real-time constraints.</description>
<maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author>