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

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;