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

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