i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +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

137
corelib/include/rtabmap/core/Graph.h
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

8
corelib/include/rtabmap/core/Parameters.h
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.");

6
corelib/include/rtabmap/core/Rtabmap.h
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;

8
corelib/include/rtabmap/core/Transform.h
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_;

15
corelib/src/CMakeLists.txt
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
####################################

846
corelib/src/Graph.cpp
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,

64
corelib/src/Rtabmap.cpp
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());
}

36
corelib/src/Transform.cpp
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));
}
}

441
corelib/src/toro3d/posegraph2.cpp Normal file
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

110
corelib/src/toro3d/posegraph2.hh Normal file
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

410
corelib/src/toro3d/transformation2.hh Normal file
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

366
corelib/src/toro3d/treeoptimizer2.cpp Normal file
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

107
corelib/src/toro3d/treeoptimizer2.hh Normal file
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

129
corelib/src/util3d.cpp
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;