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

Camera refactoring (#315)

Browse Source 
* Camera Refactoring part 1 (Mac OS X)

* fixed camera*** -> Camera***

* Fixed build for cameras Zed/RealSense/RealSense2

* increased version to 0.17.7

* fixed build for cameras K4W2 and FlyCapture2
This commit is contained in:
matlabbe
2018-10-01 19:33:56 -04:00
committed by GitHub
parent eb38b9cfab
commit 0059a4bc1b
116 changed files with 7716 additions and 6799 deletions
Download Patch File Download Diff File Expand all files Collapse all files

94
corelib/src/optimizer/toro3d/dmatrix.hh Normal file
View File

@@ -0,0 +1,94 @@
#ifndef DMATRIX_HXX
#define DMATRIX_HXX
#include <iostream>
#include <exception>
class DNotInvertibleMatrixException: public std::exception {};
class DIncompatibleMatrixException: public std::exception {};
class DNotSquareMatrixException: public std::exception {};
template <class X> struct DVector{
public:
DVector(int n=0);
~DVector();
DVector(const DVector&);
DVector& operator=(const DVector&);
X& operator[](int i) {
if ((*shares)>1) detach();
return elems[i];
}
const X& operator[](int i) const { return elems[i]; }
X operator*(const DVector&) const;
DVector operator+(const DVector&) const;
DVector operator-(const DVector&) const;
DVector operator*(const X&) const;
int dim() const { return size; }
void detach();
static DVector<X> I(int);
protected:
X * elems;
int size;
int * shares;
};
template <class X> class DMatrix {
public:
DMatrix(int n=0,int m=0);
~DMatrix();
DMatrix(const DMatrix&);
DMatrix& operator=(const DMatrix&);
X * operator[](int i) {
if ((*shares)>1) detach();
return mrows[i];
}
const X * operator[](int i) const { return mrows[i]; }
const X det() const;
DMatrix inv() const;
DMatrix transpose() const;
DMatrix operator*(const DMatrix&) const;
DMatrix operator+(const DMatrix&) const;
DMatrix operator-(const DMatrix&) const;
DMatrix operator*(const X&) const;
int rows() const { return nrows; }
int columns() const { return ncols; }
void detach();
static DMatrix I(int);
protected:
X * elems;
int nrows,ncols;
X ** mrows;
int * shares;
};
template <class X> DVector<X> operator * (const DMatrix<X> m, const DVector<X> v);
template <class X> DVector<X> operator * (const DVector<X> v, const DMatrix<X> m);
/*************** IMPLEMENTATION ***************/
#include "dmatrix.hxx"
#endif

289
corelib/src/optimizer/toro3d/dmatrix.hxx Normal file
View File

@@ -0,0 +1,289 @@
template <class X> DVector<X>::DVector(int n) {
if (n<1) n=1;
size=n;
elems=new X[size];
for (int i=0;i<size; i++)
elems[i]=X(0);
shares=new int;
(*shares)=1;
}
template <class X> DVector<X>::~DVector() {
if (--(*shares)) return;
delete [] elems;
delete shares;
}
template <class X> DVector<X>::DVector(const DVector<X>& m) {
shares=m.shares;
elems=m.elems;
size=m.size;
(*shares)++;
}
template <class X> DVector<X>& DVector<X>::operator=(const DVector<X>& m) {
if (shares==m.shares)
return *this;
if (!--(*shares)) {
delete [] elems;
delete shares;
}
shares=m.shares;
elems=m.elems;
size=m.size;
(*shares)++;
return *this;
}
template <class X> X DVector<X>::operator*(const DVector<X>& v) const{
if (size!=v.size) throw DIncompatibleMatrixException();
X p=X(0);
for (int i=0; i<size; i++)
p+=elems[i]*v.elems[i];
return p;
}
template <class X> void DVector<X>::detach() {
DVector<X> aux(size);
for (int i=0;i<size;i++) aux.elems[i]=elems[i];
operator=(aux);
}
template <class X> DVector<X> DVector<X>::operator+(const DVector<X>& v) const{
if (size!=v.size) throw DIncompatibleMatrixException();
DVector<X> r(size);
for (int i=0; i<size; i++){
r.elems[i]=elems[i]+v.elems[i];
}
return r;
}
template <class X> DVector<X> DVector<X>::operator-(const DVector<X>& v) const{
if (size!=v.size) throw DIncompatibleMatrixException();
DVector<X> r(size);
for (int i=0; i<size; i++){
r.elems[i]=elems[i]-v.elems[i];
}
return r;
}
template <class X> DVector<X> DVector<X>::operator*(const X& d) const{
DVector<X> r(size);
for (int i=0; i<size; i++){
r.elems[i]=elems[i]*d;
}
return r;
}
template <class X> DMatrix<X>::DMatrix(int n,int m) {
if (n<1) n=1;
if (m<1) m=1;
nrows=n;
ncols=m;
elems=new X[nrows*ncols];
mrows=new X* [nrows];
for (int i=0;i<nrows;i++) mrows[i]=elems+ncols*i;
for (int i=0;i<nrows*ncols;i++) elems[i]=X(0);
shares=new int;
(*shares)=1;
}
template <class X> DMatrix<X>::~DMatrix() {
if (--(*shares)) return;
delete [] elems;
delete [] mrows;
delete shares;
}
template <class X> DMatrix<X>::DMatrix(const DMatrix& m) {
shares=m.shares;
elems=m.elems;
nrows=m.nrows;
ncols=m.ncols;
mrows=m.mrows;
(*shares)++;
}
template <class X> DMatrix<X>& DMatrix<X>::operator=(const DMatrix& m) {
if (shares==m.shares)
return *this;
if (!--(*shares)) {
delete [] elems;
delete [] mrows;
delete shares;
}
shares=m.shares;
elems=m.elems;
nrows=m.nrows;
ncols=m.ncols;
mrows=m.mrows;
(*shares)++;
return *this;
}
template <class X> DMatrix<X> DMatrix<X>::inv() const {
if (nrows!=ncols) throw DNotInvertibleMatrixException();
DMatrix<X> aux1(*this),aux2(I(nrows));
aux1.detach();
for (int i=0;i<nrows;i++) {
int k=i;
for (;k<nrows&&aux1.mrows[k][i]==X(0);k++){};
if (k>=nrows) throw DNotInvertibleMatrixException();
X val=aux1.mrows[k][i];
for (int j=0;j<nrows;j++) {
aux1.mrows[k][j]=aux1.mrows[k][j]/val;
aux2.mrows[k][j]=aux2.mrows[k][j]/val;
}
if (k!=i) {
for (int j=0;j<nrows;j++) {
X tmp=aux1.mrows[k][j];
aux1.mrows[k][j]=aux1.mrows[i][j];
aux1.mrows[i][j]=tmp;
tmp=aux2.mrows[k][j];
aux2.mrows[k][j]=aux2.mrows[i][j];
aux2.mrows[i][j]=tmp;
}
}
for (int j=0;j<nrows;j++)
if (j!=i) {
X tmp=aux1.mrows[j][i];
for (int l=0;l<nrows;l++) {
aux1.mrows[j][l]=aux1.mrows[j][l]-tmp*aux1.mrows[i][l];
aux2.mrows[j][l]=aux2.mrows[j][l]-tmp*aux2.mrows[i][l];
}
}
}
return aux2;
}
template <class X> const X DMatrix<X>::det() const {
if (nrows!=ncols) throw DNotSquareMatrixException();
DMatrix<X> aux(*this);
X d=X(1);
aux.detach();
for (int i=0;i<nrows;i++) {
int k=i;
for (;k<nrows&&aux.mrows[k][i]==X(0);k++){};
if (k>=nrows) return X(0);
X val=aux.mrows[k][i];
for (int j=0;j<nrows;j++) {
aux.mrows[k][j]/=val;
}
d=d*val;
if (k!=i) {
for (int j=0;j<nrows;j++) {
X tmp=aux.mrows[k][j];
aux.mrows[k][j]=aux.mrows[i][j];
aux.mrows[i][j]=tmp;
}
d=-d;
}
for (int j=i+1;j<nrows;j++){
X tmp=aux.mrows[j][i];
if (!(tmp==X(0)) ){
for (int l=0;l<nrows;l++) {
aux.mrows[j][l]=aux.mrows[j][l]-tmp*aux.mrows[i][l];
}
//d=d*tmp;
}
}
}
return d;
}
template <class X> DMatrix<X> DMatrix<X>::transpose() const {
DMatrix<X> aux(ncols, nrows);
for (int i=0; i<nrows; i++)
for (int j=0; j<ncols; j++)
aux[j][i]=mrows[i][j];
return aux;
}
template <class X> DMatrix<X> DMatrix<X>::operator*(const DMatrix<X>& m) const {
if (ncols!=m.nrows) throw DIncompatibleMatrixException();
DMatrix<X> aux(nrows,m.ncols);
for (int i=0;i<nrows;i++)
for (int j=0;j<m.ncols;j++){
X a=0;
for (int k=0;k<ncols;k++)
a+=mrows[i][k]*m.mrows[k][j];
aux.mrows[i][j]=a;
}
return aux;
}
template <class X> DMatrix<X> DMatrix<X>::operator+(const DMatrix<X>& m) const {
if (ncols!=m.ncols||nrows!=m.nrows) throw DIncompatibleMatrixException();
DMatrix<X> aux(nrows,ncols);
for (int i=0;i<nrows*ncols;i++) aux.elems[i]=elems[i]+m.elems[i];
return aux;
}
template <class X> DMatrix<X> DMatrix<X>::operator-(const DMatrix<X>& m) const {
if (ncols!=m.ncols||nrows!=m.nrows) throw DIncompatibleMatrixException();
DMatrix<X> aux(nrows,ncols);
for (int i=0;i<nrows*ncols;i++) aux.elems[i]=elems[i]-m.elems[i];
return aux;
}
template <class X> DMatrix<X> DMatrix<X>::operator*(const X& e) const {
DMatrix<X> aux(nrows,ncols);
for (int i=0;i<nrows*ncols;i++) aux.elems[i]=elems[i]*e;
return aux;
}
template <class X> void DMatrix<X>::detach() {
DMatrix<X> aux(nrows,ncols);
for (int i=0;i<nrows*ncols;i++) aux.elems[i]=elems[i];
operator=(aux);
}
template <class X> DMatrix<X> DMatrix<X>::I(int n) {
DMatrix<X> aux(n,n);
for (int i=0;i<n;i++) aux[i][i]=X(1);
return aux;
}
template <class X> std::ostream& operator<<(std::ostream& os, const DMatrix<X> &m) {
os << "{";
for (int i=0;i<m.rows();i++) {
if (i>0) os << ",";
os << "{";
for (int j=0;j<m.columns();j++) {
if (j>0) os << ",";
os << m[i][j];
}
os << "}";
}
return os << "}";
}
template <class X> DVector<X> operator * (const DMatrix<X> m, const DVector<X> v){
if (v.dim()!=m.columns()) throw DIncompatibleMatrixException();
DVector<X> r(m.rows());
for (int i=0; i<m.rows(); i++){
X a=X(0);
for (int j=0; j<m.columns(); j++){
a+=m[i][j]*v[j];
}
r[i]=a;
}
return r;
}
template <class X> DVector<X> operator * (const DVector<X> v, const DMatrix<X> m){
if (v.dim()!=m.rows()) throw DIncompatibleMatrixException();
DVector<X> r(m.columns());
for (int i=0; i<m.columns(); i++){
X a=X(0);
for (int j=0; j<m.rows(); j++){
a+=m[j][i]*v[j];
}
r[i]=a;
}
return r;
}

274
corelib/src/optimizer/toro3d/posegraph.hh Normal file
View File

@@ -0,0 +1,274 @@
/**********************************************************************
*
* 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 posegraph.hh
*
* \brief The template class for the node parameters. The graph of
* poses with support to tree construction functionalities.
**/
#ifndef _TREEPOSEGRAPH_HXX_
#define _TREEPOSEGRAPH_HXX_
#include <iostream>
#include <assert.h>
#include <set>
#include <list>
#include <map>
#include <deque>
#include <vector>
#include <limits>
#include <algorithm>
namespace AISNavigation{
/** \brief A comparator class (struct) that compares the level
of two vertices if edges **/
template <class E>
struct EVComparator{
/** Comparison operator for the level **/
enum CompareMode {CompareLevel, CompareLength};
CompareMode mode;
EVComparator(){
mode=CompareLevel;
}
inline bool operator() (const E& e1, const E& e2){
int o1=0, o2=0;
switch (mode){
case CompareLevel:
o1=e1->top->level;
o2=e2->top->level;
break;
case CompareLength:
o1=e1->length;
o2=e2->length;
break;
}
return o1<o2;
}
};
/** \brief The template class for representing an abstract tree
without specifing the dimensionality of the exact parameterization
of the nodes. This definition is passed in via the Operation (Ops)
template class **/
template <class Ops>
struct TreePoseGraph{
typedef typename Ops::BaseType BaseType;
typedef typename Ops::PoseType Pose;
typedef typename Ops::RotationType Rotation;
typedef typename Ops::TranslationType Translation;
typedef typename Ops::TransformationType Transformation;
typedef typename Ops::CovarianceType Covariance;
typedef typename Ops::InformationType Information;
typedef typename Ops::ParametersType Parameters;
struct Vertex;
/** \brief Definition of an edge in the graph based on the template
input from Ops **/
struct Edge{
Vertex* v1; /**< The constraint is defined between v1 and v2 **/
Vertex* v2; /**< The constraint is defined between v1 and v2 **/
Vertex* top; /**< The node with the smallest level in the path **/
int length; /**< Length of the path on the tree (number of vertieces involved) **/
Transformation transformation; /**< Transformation describing the constraint (relative mapping) **/
Information informationMatrix; /**< Uncertainty encoded in the information matrix **/
bool mark;
double learningRate;
};
typedef typename EVComparator<Edge*>::CompareMode EdgeCompareMode;
typedef typename std::list< Edge* > EdgeList;
typedef typename std::map< int, Vertex* > VertexMap;
typedef typename std::set< Vertex* > VertexSet;
typedef typename std::map< Edge*, Edge* > EdgeMap;
typedef typename std::multiset< Edge*, EVComparator<Edge*> > EdgeSet;
/** \brief Definition of a vertex in the graph based on the
template input from Ops **/
struct Vertex {
// Graph-related elements
int id; /**< Id of the vertex in the graph **/
EdgeList edges; /**< The edges related to this vertex **/
// Tree-related elements
int level; /**< level in the tree. It is the distance on the tree to the root **/
Vertex* parent; /**< Parent vertex **/
Edge* parentEdge; /**< Constraint between the parent and the current vertex in the tree **/
EdgeList children; /**< All constraints involving the children of this vertex **/
// Parameterization-related elements
Transformation transformation; /**< redundant representation of the vertex, without gymbal locks **/
Pose pose; /**< The pose of the vertex **/
Parameters parameters; /**< The parameter representation **/
bool mark;
};
/** Returns the vertex with the given id **/
Vertex* vertex(int id);
/** Returns a const pointer to the vertex with the given id **/
const Vertex* vertex (int id) const;
/** Returns the edge between the two vertices **/
Edge* edge(int id1, int id2);
/** Returns a const pointer tothe edge between the two vertices **/
const Edge* edge(int id1, int id2) const;
/** Add a vertex to the graph **/
Vertex* addVertex(int id, const Pose& pose);
/** Remove a vertex from the graph **/
Vertex* removeVertex (int id);
/** Add an edge/constraint to the graph **/
Edge* addEdge(Vertex* v1, Vertex* v2, const Transformation& t, const Information& i);
/** Remove an edge/constraint from the graph **/
Edge* removeEdge(Edge* eq);
/** Adds en edge incrementally to the tree.
It builds a simple tree and initializes the structures for the optimization.
This function is for online processing.
It requires that at least one vertex is already present in the graph.
The vertices are represented by their ids.
Once the edge is introduced in the structure:
- the parent of the node with the higher ID is computed.
- the top node is assigned
- the edge is inserted in the
@returns A pointer to the added edge, if the insertion was succesfull. 0 otherwise.
**/
Edge* addIncrementalEdge(int id1, int id2, const Transformation& t, const Information& i);
/** Returns a set of edges which are accected by the mofification of the vertex v.
The set is ordered according to the level of their top node.
**/
EdgeSet* affectedEdges(Vertex* v);
EdgeSet* affectedEdges(VertexSet& vl);
/** Function to perform a breadth-first visit of the nodes in the tree to carry out a specific action act**/
template <class Action>
void treeBreadthVisit(Action& act);
/** Function to perform a depth-first visit of the nodes in the tree to carry out a specific action act **/
template <class Action>
void treeDepthVisit(Action& act, Vertex *v);
/** Constructs the tree be computing a minimal spanning tree **/
bool buildMST(int id);
/** Constructs the incremental tree according to the input trajectory **/
bool buildSimpleTree();
/** Trun around an edge (used to ensure a certain oder on the vertexes) **/
void revertEdge(Edge* e);
/** Revert edge info. This function needs to be implemented by a subclass **/
virtual void revertEdgeInfo(Edge* e) = 0;
/** Revert edge info. This function needs to be implemented by a subclass **/
virtual void initializeFromParentEdge(Vertex* v) = 0;
/** Delete all edges and vertices **/
void clear();
/**constructor*/
TreePoseGraph(){
sortedEdges=0;
edgeCompareMode=EVComparator<Edge*>::CompareLevel;
}
/** Destructor **/
virtual ~TreePoseGraph();
/** Sort constraints for correct processing order **/
EdgeSet* sortEdges();
/** Determines the length of the longest path in the tree **/
int maxPathLength();
/** Determines the path length of all pathes in the tree **/
int totalPathLength();
/** remove gaps in the indices of the vertex ids **/
void compressIndices();
/** compute the highest index of an vertex **/
int maxIndex();
/** performs a consistency check on the tree and the graph structure.
@returns false on failure.*/
bool sanityCheck();
/** The root node of the tree **/
Vertex* root;
/** All vertices **/
VertexMap vertices;
/** All edges **/
EdgeMap edges;
/** The constraints/edges sorted according to the level in the tree
in order to allow us the efficient update (pose computation) of
the nodes in the tree (see the RSS07 paper for further
details) **/
EdgeSet* sortedEdges;
protected:
void fillEdgeInfo(Edge* e);
void fillEdgesInfo();
EdgeCompareMode edgeCompareMode;
};
//include the template implementation part
#include "posegraph.hxx"
}; //namespace AISNavigation
#endif

693
corelib/src/optimizer/toro3d/posegraph.hxx Normal file
View File

@@ -0,0 +1,693 @@
/**********************************************************************
*
* 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 posegraph.hxx
*
* \brief The implementation of the template class for the node
* parameters.
**/
/*********************** IMPLEMENTATION PART ***********************/
template <typename Ops>
typename TreePoseGraph<Ops>::Vertex* TreePoseGraph<Ops>::vertex(int id){
typename VertexMap::iterator it=vertices.find(id);
if (it==vertices.end())
return 0;
return it->second;
}
template <typename Ops>
const typename TreePoseGraph<Ops>::Vertex * TreePoseGraph<Ops>::vertex (int id) const{
typename VertexMap::const_iterator it=vertices.find(id);
if (it==edges.end())
return 0;
return it->second;
}
template <class Ops>
typename TreePoseGraph<Ops>::Edge* TreePoseGraph<Ops>::edge(int id1, int id2){
Vertex* v1=vertex(id1);
if (!v1)
return 0;
typename EdgeList::iterator it=v1->edges.begin();
while(it!=v1->edges.end()){
if ((*it)->v1->id==id1 && (*it)->v2->id==id2)
return *it;
it++;
}
return 0;
}
template <class Ops>
const typename TreePoseGraph<Ops>::Edge * TreePoseGraph<Ops>::edge(int id1, int id2) const{
const Vertex* v1=vertex(id1);
if (!v1)
return false;
typename EdgeList::const_iterator it=v1->edges.begin();
while(it!=v1->edges.end()){
if ((*it)->v1->id==id1 && (*it)->v2->id==id2)
return *it;
it++;
}
return 0;
}
template <class Ops>
void TreePoseGraph<Ops>::revertEdge(typename TreePoseGraph<Ops>::Edge * e){
revertEdgeInfo(e);
Vertex* ap=e->v2;
e->v2=e->v1;
e->v1=ap;
}
template <class Ops>
typename TreePoseGraph<Ops>::Vertex* TreePoseGraph<Ops>::addVertex(int id, const typename TreePoseGraph<Ops>::Pose& pose){
Vertex* v=vertex(id);
if (v)
return 0;
v=new Vertex;
v->id=id;
v->pose=pose;
v->parent=0;
v->mark=false;
vertices.insert(std::make_pair(id,v));
return v;
}
template <class Ops>
typename TreePoseGraph<Ops>::Vertex* TreePoseGraph<Ops>::removeVertex (int id){
typename VertexMap::iterator it=vertices.find(id);
if (it==vertices.end())
return 0;
Vertex* v=it->second;
if (v==0)
return 0;
typename TreePoseGraph<Ops>::EdgeList el=v->edges;
for(typename EdgeList::iterator it=el.begin(); it!=el.end(); it++){
removeEdge(*it);
}
delete v;
vertices.erase(it);
return v;
}
template <class Ops>
typename TreePoseGraph<Ops>::Edge* TreePoseGraph<Ops>::addEdge(typename TreePoseGraph<Ops>::Vertex* v1, typename TreePoseGraph<Ops>::Vertex* v2,
const typename TreePoseGraph<Ops>::Transformation& t, const typename TreePoseGraph<Ops>::Information& i){
if (v1==v2)
return 0;
Edge* e=edge(v1->id, v2->id);
if (e)
return 0;
e=new Edge;
e->mark=false;
e->v1=v1;
e->v2=v2;
e->top=0;
e->transformation=t;
e->informationMatrix=i;
v1->edges.push_back(e);
v2->edges.push_back(e);
edges.insert(std::make_pair(e,e));
return e;
}
template <class Ops>
typename TreePoseGraph<Ops>::Edge* TreePoseGraph<Ops>::addIncrementalEdge(int id1, int id2,
const typename TreePoseGraph<Ops>::Transformation& t, const typename TreePoseGraph<Ops>::Information& i){
EVComparator<Edge*> comp;
comp.mode=edgeCompareMode;
if (! sortedEdges)
sortedEdges=new EdgeSet(comp);
typename VertexMap::iterator it1=vertices.find(id1);
typename VertexMap::iterator it2=vertices.find(id2);
Vertex* v1, *v2, *addedVertex=0;
if (it1==vertices.end() && it2==vertices.end()){
return 0;
}
if (it1==vertices.end()){
typename TreePoseGraph<Ops>::Pose p;
v1=addedVertex=addVertex(id1,p);
} else {
v1=it1->second;
}
if (it2==vertices.end()){
typename TreePoseGraph<Ops>::Pose p;
v2=addedVertex=addVertex(id2,p);
} else {
v2=it2->second;
}
if (v1->id==v2->id){
assert(0);
}
Edge* e=addEdge(v1,v2,t,i);
if (!e){
return 0;
}
if (v1->id>v2->id)
revertEdge(e);
if (addedVertex){
Vertex* otherVertex= (addedVertex==v1)? v2:v1;
addedVertex->parent=otherVertex;
addedVertex->parentEdge=e;
addedVertex->level=otherVertex->level+1;
otherVertex->children.push_back(e);
}
fillEdgeInfo(e);
sortedEdges->insert(e);
if (addedVertex){
initializeFromParentEdge(addedVertex);
}
return e;
}
template <class Ops>
typename TreePoseGraph<Ops>::Edge* TreePoseGraph<Ops>::removeEdge(typename TreePoseGraph<Ops>::Edge* e){
{
typename EdgeMap::iterator it=edges.find(e);
if (it==edges.end()){
return 0;
}
edges.erase(it);
}
Vertex* v1=e->v1;
Vertex* v2=e->v2;
{
typename EdgeList::iterator it=v1->edges.begin();
while(it!=v1->edges.end()){
if (*it==e){
v1->edges.erase(it);
break;
}
it++;
}
}
{
typename EdgeList::iterator it=v2->edges.begin();
while(it!=v2->edges.end()){
if ((*it)==e){
delete *it;
v2->edges.erase(it);
break;
}
it++;
}
}
return e;
}
template <class Ops>
template <class Action>
void TreePoseGraph<Ops>::treeBreadthVisit(Action& act){
typedef std::deque<Vertex*> VertexDeque;
static VertexDeque q;
q.push_back(root);
while (!q.empty()){
Vertex* current=q.front();
act.perform(current);
q.pop_front();
typename EdgeList::iterator it=current->children.begin();
while(it!=current->children.end()){
typename TreePoseGraph::Edge* e=(*it);
q.push_back(e->v2);
if(e->v2==current){
std::cerr << "error in the link direction v=" << current->id << std::endl;
std::cerr << " v1=" << e->v1->id << " v2=" << e->v2->id << std::endl;
assert(0);
}
it++;
}
}
q.clear();
}
template <class Ops>
template <class Action>
void TreePoseGraph<Ops>::treeDepthVisit(Action& act, Vertex* v){
act.perform(v);
typename EdgeList::iterator it=v->children.begin();
while(it!=v->children.end()){
treeDepthVisit(act, (*it)->v2);
it++;
}
}
template <class Ops>
bool TreePoseGraph<Ops>::buildMST(int id){
typedef std::deque<Vertex*> VertexDeque;
typename VertexMap::iterator it=vertices.begin();
while (it!=vertices.end()){
it->second->parent=0;
it->second->parentEdge=0;
it->second->children.clear();
it++;
}
Vertex* v=vertex(id);
if (!v)
return false;
root=v;
root->level=0;
VertexDeque q;
q.push_back(v);
//std::cerr << "v=" << v->id << std::endl;
while (!q.empty()){
v=q.front();
typename EdgeList::iterator it=v->edges.begin();
while (it!=v->edges.end()){
Edge* e=(*it);
bool invertedEdge=false;
Vertex* other=e->v2;
if (other==v){
other=e->v1;
invertedEdge=true;
}
if (other!=root && other->parent==0){
if (invertedEdge){
revertEdge(e);
}
//std::cerr << "INSERT v=" << v->id<< " " << "e=(" << e->v1->id << "," << e->v2->id << ")" << std::endl;
other->parent=v;
other->parentEdge=e;
other->level=v->level+1;
q.push_back(other);
v->children.push_back(e);
//std::cerr << "v=" << other->id << std::endl;
}
it++;
}
q.pop_front();
}
fillEdgesInfo();
return true;
}
/** \brief A class (struct) to dermine the level of a vertex in the tree **/
template <class TPG>
struct LevelAssigner{
/** Dermines the level of the vertex v in the tree **/
void perform(typename TPG::Vertex* v){
if (v->parent)
v->level=v->parent->level+1;
else
v->level=0;
}
};
template <class Ops>
bool TreePoseGraph<Ops>::buildSimpleTree(){
root=0;
//rectify all the constraints, so that the v1<v2
for (typename EdgeMap::iterator it=edges.begin(); it!=edges.end(); it++){
Edge* e=it->second;
if (e->v1->id > e->v2->id)
revertEdge(e);
}
//clear the tree data
for (typename VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
Vertex* v=it->second;
v->parent=0;
v->parentEdge=0;
v->children.clear();
}
//fill the structure
for (typename VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
Vertex* v=it->second;
if (v->edges.empty()){
assert(0);
continue;
}
Edge* bestEdge=v->edges.front();
int bestId=std::numeric_limits<int>::max();
bool found=false;
typename EdgeList::iterator li=v->edges.begin();
while(li!=v->edges.end()){
Edge* e =*li;
if (e->v2==v && e->v1->id<bestId){ //consider only the entering edges
bestId=e->v1->id;
bestEdge=e;
found=true;
}
li++;
}
if (found){
v->parentEdge=bestEdge;
v->parent=bestEdge->v1;
v->parent->children.push_back(bestEdge);
} else {
assert(! root);
root=v;
}
}
// std::cerr << "root=" << root << std::endl;
assert(root);
//assign the level
LevelAssigner< TreePoseGraph<Ops> > oa;
treeDepthVisit(oa, root);
fillEdgesInfo();
return true;
}
template <class Ops>
TreePoseGraph<Ops>::~TreePoseGraph(){
clear();
}
template <class Ops>
void TreePoseGraph<Ops>::clear(){
for (typename VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
delete it->second;
it->second=0;
}
for (typename EdgeMap::iterator it=edges.begin(); it!=edges.end(); it++){
delete it->second;
it->second=0;
}
vertices.clear();
edges.clear();
if ( sortedEdges )
delete sortedEdges;
sortedEdges=0;
}
template <class Ops>
void TreePoseGraph<Ops>::fillEdgeInfo(Edge* e){
Vertex* v1=e->v1;
Vertex* v2=e->v2;
int length=0;
while (v1!=v2) {
if (v1->level > v2->level){
v1=v1->parent;
length++;
} else if (v2->level > v1->level){
v2=v2->parent;
length++;
} else if (v1->level==v2->level){
v1=v1->parent;
v2=v2->parent;
length+=2;
}
}
e->length=length;
e->top=v1;
}
template <class Ops>
void TreePoseGraph<Ops>::fillEdgesInfo(){
typename TreePoseGraph<Ops>::EdgeMap em=edges;
for(typename EdgeMap::iterator it=em.begin(); it!=em.end(); it++){
fillEdgeInfo(it->second);
}
}
template <class Ops>
typename TreePoseGraph<Ops>::EdgeSet* TreePoseGraph<Ops>::sortEdges(){
EVComparator<Edge*> comp;
comp.mode=edgeCompareMode;
EdgeSet * el=new EdgeSet(comp);
typename EdgeMap::iterator it=edges.begin();
while(it!=edges.end()){
el->insert(it->second);
it++;
}
return el;
}
template <class Ops>
typename TreePoseGraph<Ops>::EdgeSet* TreePoseGraph<Ops>::affectedEdges(Vertex* v){
EVComparator<Edge*> comp;
comp.mode=edgeCompareMode;
EdgeSet * es=new EdgeSet(comp);
std::deque<Vertex*> frontier;
std::list<Vertex*> markedVertices;
//frontier.push_back(v);
//v->mark=true;
for (typename EdgeList::iterator it=v->children.begin(); it!=v->children.end(); it++){
Edge* e=*it;
Vertex* other=(e->v1==v)?e->v2:e->v1;
frontier.push_back(other);
other->mark=true;
markedVertices.push_back(other);
e->mark=true;
es->insert(e);
}
while (! frontier.empty()){
Vertex* c=frontier.front();
frontier.pop_front();
markedVertices.push_back(c);
EdgeList& el=c->edges;
for (typename EdgeList::iterator it=el.begin(); it!=el.end(); it++){
Edge* e=*it;
if (e->mark)
continue;
Vertex* other= (e->v1==c)?e->v2:e->v1;
if (other==c->parent)
continue;
if (other!=e->top && ! e->top->mark){
e->top->mark=true;
frontier.push_back(e->top);
}
e->mark=true;
es->insert(e);
if (!other->mark){
other->mark=true;
frontier.push_back(other);
}
}
}
for (typename std::list<Vertex*>::iterator it=markedVertices.begin(); it!=markedVertices.end(); it++){
(*it)->mark=false;
}
for (typename EdgeSet::iterator it=es->begin(); it!=es->end(); it++){
(*it)->mark=false;
}
return es;
}
template <class Ops>
typename TreePoseGraph<Ops>::EdgeSet* TreePoseGraph<Ops>::affectedEdges(typename TreePoseGraph<Ops>::VertexSet& vl){
EVComparator<Edge*> comp;
comp.mode=edgeCompareMode;
EdgeSet * es=new EdgeSet(comp);
std::deque<Vertex*> frontier;
std::list<Vertex*> markedVertices;
// for (typename VertexSet::iterator it=vl.begin(); it!=vl.end(); it++){
// frontier.push_back(*it);
// (*it)->mark=true;
// }
for (typename VertexSet::iterator it=vl.begin(); it!=vl.end(); it++){
Vertex* v=*it;
for (typename EdgeList::iterator it=v->children.begin(); it!=v->children.end(); it++){
Edge* e=*it;
Vertex* other=(e->v1==v)?e->v2:e->v1;
frontier.push_back(other);
other->mark=true;
markedVertices.push_back(other);
e->mark=true;
es->insert(e);
}
}
while (! frontier.empty()){
Vertex* c=frontier.front();
frontier.pop_front();
markedVertices.push_back(c);
EdgeList& el=c->edges;
for (typename EdgeList::iterator it=el.begin(); it!=el.end(); it++){
Edge* e=*it;
if (e->mark)
continue;
Vertex* other= (e->v1==c)?e->v2:e->v1;
if (other==c->parent)
continue;
if (other!=e->top && ! e->top->mark){
e->top->mark=true;
frontier.push_back(e->top);
}
e->mark=true;
es->insert(e);
if (!other->mark){
other->mark=true;
frontier.push_back(other);
}
}
}
for (typename std::list<Vertex*>::iterator it=markedVertices.begin(); it!=markedVertices.end(); it++){
(*it)->mark=false;
}
for (typename EdgeSet::iterator it=es->begin(); it!=es->end(); it++){
(*it)->mark=false;
}
return es;
}
template <class Ops>
int TreePoseGraph<Ops>::maxPathLength(){
int max=0;
typename EdgeMap::const_iterator it=edges.begin();
while(it!=edges.end()){
int l=it->second->length;
max=l>max?l:max;
it++;
}
return max;
}
template <class Ops>
int TreePoseGraph<Ops>::totalPathLength(){
int t=0;
typename EdgeMap::const_iterator it=edges.begin();
while(it!=edges.end()){
t+=it->second->length;
it++;
}
return t;
}
template <class Ops>
void TreePoseGraph<Ops>::compressIndices(){
VertexMap vmap;
int i=0;
for (typename VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
Vertex* v=it->second;
v->id=i;
vmap.insert(std::make_pair(i,v));
i++;
}
vertices=vmap;
}
template <class Ops>
int TreePoseGraph<Ops>::maxIndex(){
typename VertexMap::reverse_iterator it=vertices.rbegin();
if (it!=vertices.rend())
return it->second->id;
return -1;
}
template <class TPG>
struct LoopChecker{
bool noloops;
void perform(typename TPG::Vertex* v){
if (!noloops)
return;
if (!v->mark)
v->mark=true;
else
noloops=false;
}
};
template <class Ops>
bool TreePoseGraph<Ops>::sanityCheck(){
//check that each node has exactly one parent
for (typename VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
Vertex* v=it->second;
v->mark=false;
Vertex* vp=v->parent;
if (! vp){
if (v!=root){
std::cerr << "root not found in the graph" << std::endl;
return false;
}
}
const EdgeList& children=it->second->children;
for (typename EdgeList::const_iterator lt=children.begin(); lt!=children.end(); lt++){
if ((*lt)->v1!=v){
std::cerr << "wrong direction of the edges" << std::endl;
return false;
}
}
}
//check that there are no loops in the tree
LoopChecker< TreePoseGraph<Ops> > lc;
lc.noloops=true;
treeBreadthVisit(lc);
if (!lc.noloops){
std::cerr << "the tree contains loops" << std::endl;
return false;
}
for (typename VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
Vertex* v=it->second;
v->mark=false;
}
return true;
}

441
corelib/src/optimizer/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 {
typedef unsigned int uint;
#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();
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);
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(vertices.find(e->v1->id)!=vertices.end());
assert(vertices.find(e->v2->id)!=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/optimizer/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

404
corelib/src/optimizer/toro3d/posegraph3.cpp Normal file
View File

@@ -0,0 +1,404 @@
/**********************************************************************
*
* 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 posegraph3.cpp
*
* \brief Defines the graph of 3D poses, with specific functionalities
* such as loading, saving, merging constraints, and etc.
**/
#include "posegraph3.hh"
#include <fstream>
#include <sstream>
#include <string>
using namespace std;
namespace AISNavigation {
#define LINESIZE 81920
//#define DEBUG(i) if (verboseLevel>i) cerr
bool TreePoseGraph3::load(const char* filename, bool overrideCovariances, bool twoDimensions){
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 (twoDimensions){
if (tag=="VERTEX"){
int id;
Pose p(0.,0.,0.,0.,0.,0.);
ls >> id >> p.x() >> p.y() >> p.yaw();
TreePoseGraph3::Vertex* v=addVertex(id,p);
if (v){
v->transformation=Transformation(p);
}
}
} else {
if (tag=="VERTEX3"){
int id;
Pose p;
ls >> id >> p.x() >> p.y() >> p.z() >> p.roll() >> p.pitch() >> p.yaw();
TreePoseGraph3::Vertex* v=addVertex(id,p);
if (v){
v->transformation=Transformation(p);
}
}
}
}
is.clear(); /* clears the end-of-file and error flags */
is.seekg(0, ios::beg);
//bool edgesOk=true;
while(is){
char buf[LINESIZE];
is.getline(buf,LINESIZE);
istringstream ls(buf);
string tag;
ls >> tag;
if (twoDimensions){
if (tag=="EDGE"){
int id1, id2;
Pose p(0.,0.,0.,0.,0.,0.);
InformationMatrix m;
ls >> id1 >> id2 >> p.x() >> p.y() >> p.yaw();
m=DMatrix<double>::I(6);
if (! overrideCovariances){
ls >> m[0][0] >> m[0][1] >> m[1][1] >> m[2][2] >> m[0][2] >> m[1][2];
m[2][0]=m[0][2]; m[2][1]=m[1][2]; m[1][0]=m[0][1];
}
TreePoseGraph3::Vertex* v1=vertex(id1);
TreePoseGraph3::Vertex* v2=vertex(id2);
Transformation t(p);
if (!addEdge(v1, v2,t ,m)){
cerr << "Fatal, attempting to insert an edge between non existing nodes, skipping";
cerr << "edge=" << id1 <<" -> " << id2 << endl;
//edgesOk=false;
}
}
} else {
if (tag=="EDGE3"){
int id1, id2;
Pose p;
InformationMatrix m;
ls >> id1 >> id2 >> p.x() >> p.y() >> p.z() >> p.roll() >> p.pitch() >> p.yaw();
m=DMatrix<double>::I(6);
if (! overrideCovariances){
for (int i=0; i<6; i++)
for (int j=i; j<6; j++)
ls >> m[i][j];
}
TreePoseGraph3::Vertex* v1=vertex(id1);
TreePoseGraph3::Vertex* v2=vertex(id2);
Transformation t(p);
if (!addEdge(v1, v2,t ,m)){
cerr << "Fatal, attempting to insert an edge between non existing nodes, skipping";
cerr << "edge=" << id1 <<" -> " << id2 << endl;
//edgesOk=false;
}
}
}
}
return true;
//return edgesOk;
}
bool TreePoseGraph3::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);
}
return true;
}
bool TreePoseGraph3::saveGnuplot(const char* filename){
ofstream os(filename);
if (!os)
return false;
for (TreePoseGraph3::VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
TreePoseGraph3::Vertex* v=it->second;
v->pose=v->transformation.toPoseType();
}
for (TreePoseGraph3::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
const TreePoseGraph3::Edge * e=it->second;
const Vertex* v1=e->v1;
const Vertex* v2=e->v2;
os << v1->pose.x() << " " << v1->pose.y() << " " << v1->pose.z() << " "
<< v1->pose.roll() << " " << v1->pose.pitch() << " " << v1->pose.yaw() <<endl;
os << v2->pose.x() << " " << v2->pose.y() << " " << v2->pose.z() << " "
<< v2->pose.roll() << " " << v2->pose.pitch() << " " << v2->pose.yaw() <<endl;
os << endl << endl;
}
return true;
}
bool TreePoseGraph3::save(const char* filename){
ofstream os(filename);
if (!os)
return false;
for (TreePoseGraph3::VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
TreePoseGraph3::Vertex* v=it->second;
v->pose=v->transformation.toPoseType();
os << "VERTEX3 "
<< v->id << " "
<< v->pose.x() << " "
<< v->pose.y() << " "
<< v->pose.z() << " "
<< v->pose.roll() << " "
<< v->pose.pitch() << " "
<< v->pose.yaw() << endl;
}
for (TreePoseGraph3::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
const TreePoseGraph3::Edge * e=it->second;
os << "EDGE3 " << e->v1->id << " " << e->v2->id << " ";
Pose p=e->transformation.toPoseType();
os << p.x() << " " << p.y() << " " << p.z() << " " << p.roll() << " " << p.pitch() << " " << p.yaw() << " ";
for (int i=0; i<6; i++)
for (int j=i; j<6; j++)
os << e->informationMatrix[i][j] << " ";
os << 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(TreePoseGraph3::Vertex* v){
std::cout << "(" << v->id << "," << v->level << ")" << endl;
}
};
void TreePoseGraph3::printDepth( std::ostream& os ){
IdPrinter ip(os);
treeDepthVisit(ip, root);
}
void TreePoseGraph3::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(TreePoseGraph3::Vertex* v){
if (!v->parent)
return;
TreePoseGraph3::Transformation tParent(v->parent->transformation);
TreePoseGraph3::Transformation tNode=tParent*v->parentEdge->transformation;
assert(v->parentEdge->v1==v->parent);
assert(v->parentEdge->v2==v);
v->transformation=tNode;
}
};
void TreePoseGraph3::initializeOnTree(){
PosePropagator pp;
treeDepthVisit(pp, root);
}
void TreePoseGraph3::printEdgesStat(std::ostream& os){
for (TreePoseGraph3::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
const TreePoseGraph3::Edge * e=it->second;
os << "EDGE " << e->v1->id << " " << e->v2->id << " ";
Pose p=e->transformation.toPoseType();
os << p.x() << " " << p.y() << " " << p.z() << " " << p.roll() << " " << p.pitch() << " " << p.yaw() << endl;
os << " top=" << e->top->id << " length=" << e->length << endl;
}
}
void TreePoseGraph3::revertEdgeInfo(Edge* e){
// here we assume uniform covariances, and we neglect the transofrmation
// induced by the Jacobian when reverting the link
e->transformation=e->transformation.inv();
};
void TreePoseGraph3::initializeFromParentEdge(Vertex* v){
Transformation tp=Transformation(v->parent->pose)*v->parentEdge->transformation;
v->transformation=tp;
v->pose=tp.toPoseType();
v->parameters=v->parentEdge->transformation;
}
void TreePoseGraph3::collapseEdge(Edge* e){
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;
CovarianceMatrix CM=C2x;
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;
}
}
// FIXME
// edges leading to the same node are ignored
// should be merged
if (e1x && e2x){
// here goes something for mergin the constraints, according to the information matrices.
// in 3D it is a nightmare, so i postpone this, and i simply ignore the redundant constraints.
// the resultng system is overconfident
}
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++;
}
}
void TreePoseGraph3::recomputeAllTransformations(){
TransformationPropagator tp;
treeDepthVisit(tp,root);
}
}; //namespace AISNavigation

145
corelib/src/optimizer/toro3d/posegraph3.hh Normal file
View File

@@ -0,0 +1,145 @@
/**********************************************************************
*
* 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 posegraph3.hh
*
* \brief Defines the graph of 3D poses, with specific functionalities
* such as loading, saving, merging constraints, and etc.
**/
#ifndef _POSEGRAPH3_HH_
#define _POSEGRAPH3_HH_
#include "posegraph.hh"
#include "transformation3.hh"
#include <iostream>
#include <vector>
typedef unsigned int uint;
#ifndef M_PI
#define M_PI 3.14159265359
#endif
namespace AISNavigation {
/** \brief The class (struct) that contains 2D graph related functions
such as loading, saving, merging, etc. **/
struct TreePoseGraph3: public TreePoseGraph<Operations3D<double> >{
typedef Operations3D<double> Ops;
typedef Ops::PoseType Pose;
typedef Ops::RotationType Rotation;
typedef Ops::TranslationType Translation;
typedef Ops::TransformationType Transformation;
typedef Ops::CovarianceType CovarianceMatrix;
typedef Ops::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, bool twoDimensions=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);
/** Initializes the parameters based on the topology of the tree and the actual transformation*/
void initializeOnTree();
/** Recomputes all the transformations based on the parameters and the tree*/
void recomputeAllTransformations();
virtual void initializeFromParentEdge(Vertex* v);
/** Turn around the edge (<i,j> => <j,i>) **/
virtual void revertEdgeInfo(Edge* e);
/** 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;
protected:
/** \brief A class (struct) to compute the parameterization of the vertex v **/
struct ParameterPropagator{
inline void perform(TreePoseGraph3::Vertex* v){
if (!v->parent){
v->parameters=TreePoseGraph3::Transformation(0.,0.,0.,0.,0.,0.);
return;
}
v->parameters=v->parent->transformation.inv()*v->transformation;
}
};
/** \brief A class (struct) to compute the parameterization of the vertex v **/
struct TransformationPropagator{
inline void perform(TreePoseGraph3::Vertex* v){
if (!v->parent){
return;
}
v->transformation=v->parent->transformation*v->parameters;
}
};
};
}; //namespace AISNavigation
#endif

3
corelib/src/optimizer/toro3d/readme.txt Normal file
View File

@@ -0,0 +1,3 @@
Info: https://www.openslam.org/toro.html
License: Creative Commons (Attribution-NonCommercial-ShareAlike)

412
corelib/src/optimizer/toro3d/transformation2.hh Normal file
View File

@@ -0,0 +1,412 @@
/**********************************************************************
*
* 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){
setRotation(_theta);
setTranslation(trans.x(), trans.y());
}
/** 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

275
corelib/src/optimizer/toro3d/transformation3.hh Normal file
View File

@@ -0,0 +1,275 @@
/**********************************************************************
*
* 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.
**********************************************************************/
#ifndef _TRANSFORMATION3_HXX_
#define _TRANSFORMATION3_HXX_
#include <assert.h>
#include <cmath>
#include "dmatrix.hh"
namespace AISNavigation {
template <class T>
struct Vector3 {
T elems[3] ;
Vector3(T x, T y, T z) {elems[0]=x; elems[1]=y; elems[2]=z;}
Vector3() {elems[0]=0.; elems[1]=0.; elems[2]=0.;}
Vector3(const DVector<T>& t){}
// translational view
inline const T& x() const {return elems[0];}
inline const T& y() const {return elems[1];}
inline const T& z() const {return elems[2];}
inline T& x() {return elems[0];}
inline T& y() {return elems[1];}
inline T& z() {return elems[2];}
// rotational view
inline const T& roll() const {return elems[0];}
inline const T& pitch() const {return elems[1];}
inline const T& yaw() const {return elems[2];}
inline T& roll() {return elems[0];}
inline T& pitch() {return elems[1];}
inline T& yaw() {return elems[2];}
};
template <class T>
struct Pose3 : public DVector<T>{
Pose3();
Pose3(const Vector3<T>& rot, const Vector3<T>& trans);
Pose3(const T& x, const T& y, const T& z, const T& roll, const T& pitch, const T& yaw);
Pose3(const DVector<T>& v): DVector<T>(v) {assert(v.dim()==6);}
inline operator const DVector<T>& () {return (const DVector<T>)*this;}
inline operator DVector<T>& () {return *this;}
inline const T& roll() const {return DVector<T>::elems[0];}
inline const T& pitch() const {return DVector<T>::elems[1];}
inline const T& yaw() const {return DVector<T>::elems[2];}
inline const T& x() const {return DVector<T>::elems[3];}
inline const T& y() const {return DVector<T>::elems[4];}
inline const T& z() const {return DVector<T>::elems[5];}
inline T& roll() {return DVector<T>::elems[0];}
inline T& pitch() {return DVector<T>::elems[1];}
inline T& yaw() {return DVector<T>::elems[2];}
inline T& x() {return DVector<T>::elems[3];}
inline T& y() {return DVector<T>::elems[4];}
inline T& z() {return DVector<T>::elems[5];}
};
/*!
* A Quaternion can be used to either represent a rotational axis
* and a Rotation, or, the point which will be rotated
*/
template <class T>
struct Quaternion{
/*!
* Default Constructor: w=x=y=z=0;
*/
Quaternion();
/*!
* The Quaternion representation of the point "pose"
*/
Quaternion(const Vector3<T>& pose);
/*!
* create a Quaternion by scalar w and the imaginery parts x,y, and z.
*/
Quaternion(const T _w, const T _x, const T _y, const T _z);
/*!
* create a rotational Quaternion, roll along x-axis, pitch along y-axis and yaw along z-axis
*/
Quaternion(const T _roll_x_phi, const T _pitch_y_theta, const T _yaw_z_psi);
/*!
* @return the conjugated version of this quaternion
*/
inline Quaternion<T> conjugated() const;
/*!
* @return this quaternion, but normalized
*/
inline Quaternion<T> normalized() const;
/*!
* @return the inverse of this Quaternion
*/
inline Quaternion<T> inverse() const;
/*construct a quaternion on the axis/angle representation*/
inline Quaternion(const Vector3<T>& axis, const T& angle);
/*!
* if this Quaternion represents a point, use this function
* to rotate the point along <axis> with angle <alpha>
* @param axis the rotational axis
* @param alpha rotational angle
*/
inline Quaternion<T> rotateThisAlong (const Vector3<T>& axis, const T alpha) const;
/*!
* if this Quaternion represents a rotational axis + rotation,
* use this function to rotate another point represented as a Quaternion p
* @param p the point to be rotated by <this>. Point is represented as a Quaternion
* @return rotated Point (represented as a Quaternion)
*/
inline Quaternion<T> rotatePoint(const Quaternion& p) const;
/*!
* if this Quaternion represents a rotational axis + rotation,
* use this function to rotate another point
* @param p the point to be rotated by <this>.
* @return rotated Point
*/
inline Vector3<T> rotatePoint(const Vector3<T>& p) const;
/*!
* if this Quaternion represents a rotational axis, add a rotation of angle <alpha>
* along <this> axis to the Quaternion
* @param alpha rotational value
* @return this Quaternion with included information about the rotation along <this> axis
*/
inline Quaternion withRotation (const T alpha) const;
/*!
* Given rotational axis x,y,z, get the rotation along these axis encoded in this Quaternion
* @return rotation along x,y,z axis encoded in <this> Quaternion
*/
inline Vector3<T> toAngles() const;
inline Vector3<T> axis() const;
inline T angle() const;
/*!
* @return the norm of this Quaternion
*/
inline T norm() const;
/*!
* @return the real part (==w) of this Quaternion
*/
inline T re() const;
/*!
* @return the imaginery part (== (x,y,z)) of this Quaternion
*/
inline Vector3<T> im() const;
T w,x,y,z;
};
template <class T> inline Quaternion<T> operator + (const Quaternion<T> & left, const Quaternion<T>& right);
template <class T> inline Quaternion<T> operator - (const Quaternion<T> & left, const Quaternion<T>& right);
template <class T> inline Quaternion<T> operator * (const Quaternion<T> & left, const Quaternion<T>& right);
template <class T> inline Quaternion<T> operator * (const Quaternion<T> & left, const T scalar);
template <class T> inline Quaternion<T> operator * (const T scalar, const Quaternion<T>& right);
template <class T> std::ostream& operator << (std::ostream& os, const Quaternion<T>& q);
template <class T> inline T innerproduct(const Quaternion<T>& left, const Quaternion<T>& right);
template <class T> inline Quaternion<T> slerp(const Quaternion<T>& from, const Quaternion<T>& to, const T lambda);
template <class T>
struct Transformation3{
Quaternion<T> rotationQuaternion;
Vector3<T> translationVector;
Transformation3(){}
inline static Transformation3<T> identity();
Transformation3 (const Vector3<T>& trans, const Quaternion<T>& rot);
Transformation3 (const Pose3<T>& v);
Transformation3 (const T& x, const T& y, const T& z, const T& roll, const T& pitch, const T& yaw);
inline Vector3<T> translation() const;
inline Quaternion <T> rotation() const;
inline Pose3<T> toPoseType() const;
inline void setTranslation(const Vector3<T>& t);
inline void setTranslation(const T& x, const T& y, const T& z);
inline void setRotation(const Vector3<T>& r);
inline void setRotation(const T& roll, const T& pitch, const T& yaw);
inline void setRotation(const Quaternion<T>& q);
inline Transformation3<T> inv() const;
inline bool validRotation(const T& epsilon=0.001) const;
};
template <class T>
inline Vector3<T> operator * (const Transformation3<T>& m, const Vector3<T>& v);
template <class T>
inline Transformation3<T> operator * (const Transformation3<T>& m1, const Transformation3<T>& m2);
template <class T>
struct Operations3D{
typedef T BaseType;
typedef Pose3<T> PoseType;
typedef Quaternion<T> RotationType;
typedef Vector3<T> TranslationType;
typedef Transformation3<T> TransformationType;
typedef DMatrix<T> CovarianceType;
typedef DMatrix<T> InformationType;
typedef Transformation3<T> ParametersType;
};
} // namespace AISNavigation
/**************************** IMPLEMENTATION ****************************/
#include "transformation3.hxx"
#endif

451
corelib/src/optimizer/toro3d/transformation3.hxx Normal file
View File

@@ -0,0 +1,451 @@
/**********************************************************************
*
* 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.
**********************************************************************/
#include <limits>
namespace AISNavigation {
template <class T>
inline Vector3<T> operator * (const T& d, const Vector3<T>& v) {
return Vector3<T>(v.elems[0]*d, v.elems[1]*d, v.elems[2]*d);
}
template <class T>
inline Vector3<T> operator * (const Vector3<T>& v, const T& d) {
return Vector3<T>(v.elems[0]*d, v.elems[1]*d, v.elems[2]*d);
}
template <class T>
inline T operator * (const Vector3<T>& v1, const Vector3<T>& v2){
return v1.elems[0]*v2.elems[0]
+ v1.elems[1]*v2.elems[1]
+ v1.elems[2]*v2.elems[2];
}
template <class T>
inline Vector3<T> operator + (const Vector3<T>& v1, const Vector3<T>& v2){
return Vector3<T>(v1.elems[0]+v2.elems[0],
v1.elems[1]+v2.elems[1],
v1.elems[2]+v2.elems[2]);
}
template <class T>
Vector3<T> operator - (const Vector3<T>& v1, const Vector3<T>& v2){
return Vector3<T>(v1.elems[0]-v2.elems[0],
v1.elems[1]-v2.elems[1],
v1.elems[2]-v2.elems[2]);
}
template <class T>
Pose3<T>::Pose3(): DVector<T>(6){
}
template <class T>
Pose3<T>::Pose3(const Vector3<T>& trans, const Vector3<T>& rot): DVector<T>(6){
DVector<T>::elems[0]=rot.roll();
DVector<T>::elems[1]=rot.pitch();
DVector<T>::elems[2]=rot.yaw();
DVector<T>::elems[3]=trans.x();
DVector<T>::elems[4]=trans.y();
DVector<T>::elems[5]=trans.z();
}
template <class T>
Pose3<T>::Pose3(const T& x, const T& y, const T& z, const T& r, const T& p, const T& yw): DVector<T>(6){
DVector<T>::elems[0]=r;
DVector<T>::elems[1]=p;
DVector<T>::elems[2]=yw;
DVector<T>::elems[3]=x;
DVector<T>::elems[4]=y;
DVector<T>::elems[5]=z;
}
#define MY_MAX(a,b) (((a)>(b))?(a):(b))
template<class T>
Quaternion<T>::Quaternion(){
w = 1;
x = 0;
y = 0;
z = 0;
}
template<class T>
Quaternion<T>::Quaternion(const Vector3<T>& pose){
w = 0;
x = pose.x();
y = pose.y();
z = pose.z();
}
template<class T>
Quaternion<T>::Quaternion(const Vector3<T>& axis, const T& angle){
T sa=sin(angle/2);
T ca=cos(angle/2);
w=ca;
x=axis.x()*sa;
y=axis.y()*sa;
z=axis.z()*sa;
}
template<class T>
Quaternion<T>::Quaternion(const T _w, const T _x, const T _y, const T _z){
w = _w;
x = _x;
y = _y;
z = _z;
}
template<class T>
Quaternion<T>::Quaternion(const T phi, const T theta, const T psi){
T sphi = sin(phi);
T stheta = sin(theta);
T spsi = sin(psi);
T cphi = cos(phi);
T ctheta = cos(theta);
T cpsi = cos(psi);
T _r[3][3] = { //create rotational Matrix
{cpsi*ctheta, cpsi*stheta*sphi - spsi*cphi, cpsi*stheta*cphi + spsi*sphi},
{spsi*ctheta, spsi*stheta*sphi + cpsi*cphi, spsi*stheta*cphi - cpsi*sphi},
{ -stheta, ctheta*sphi, ctheta*cphi}
};
T _w = sqrt(MY_MAX(0, 1 + _r[0][0] + _r[1][1] + _r[2][2]))/2.0;
T _x = sqrt(MY_MAX(0, 1 + _r[0][0] - _r[1][1] - _r[2][2]))/2.0;
T _y = sqrt(MY_MAX(0, 1 - _r[0][0] + _r[1][1] - _r[2][2]))/2.0;
T _z = sqrt(MY_MAX(0, 1 - _r[0][0] - _r[1][1] + _r[2][2]))/2.0;
this->w = _w;
this->x = (_r[2][1] - _r[1][2])>=0?fabs(_x):-fabs(_x);
this->y = (_r[0][2] - _r[2][0])>=0?fabs(_y):-fabs(_y);
this->z = (_r[1][0] - _r[0][1])>=0?fabs(_z):-fabs(_z);
}
template<class T>
inline Quaternion<T> Quaternion<T>::conjugated() const{
return Quaternion<T>(w,-x,-y,-z);
}
template<class T>
inline Quaternion<T> Quaternion<T>::normalized() const{
T n = this->norm();
if (n > 0)
return ((1./n) * (*this));
else
return Quaternion<T>(0.,0.,0.,0.);
}
template<class T>
inline Quaternion<T> Quaternion<T>::inverse() const{
return ((1./this->norm()) * this->conjugated());
}
template<class T>
inline Quaternion<T> Quaternion<T>::rotateThisAlong(const Vector3<T>& axis, const T alpha) const{
Quaternion<T> q(axis);
q = q.normalized();
q = q.withRotation(alpha);
return q.rotatePoint(*this);
}
template<class T>
inline Quaternion<T> Quaternion<T>::rotatePoint(const Quaternion<T>& p) const{
return (*this)*p*(this->conjugated());
}
template<class T>
inline Vector3<T> Quaternion<T>::rotatePoint(const Vector3<T>& point) const{
Quaternion<T> p(point);
Quaternion<T> q = this->rotatePoint(p);
return q.im();
}
template<class T>
inline Quaternion<T> Quaternion<T>::withRotation(const T alpha) const{
Quaternion<T> q = normalized();
T salpha = sin(alpha/2.);
T calpha = cos(alpha/2.);
q.w = calpha;
q.x = salpha * q.x;
q.y = salpha * q.y;
q.z = salpha * q.z;
return q;
}
template<class T>
inline Vector3<T> Quaternion<T>::toAngles() const{
T n = this->norm();
T s = n > 0?2./(n*n):0.;
T m00, m10, m20, m21, m22;
T phi,theta,psi;
T xs = this->x*s;
T ys = this->y*s;
T zs = this->z*s;
T wx = this->w*xs;
T wy = this->w*ys;
T wz = this->w*zs;
T xx = this->x*xs;
T xy = this->x*ys;
T xz = this->x*zs;
T yy = this->y*ys;
T yz = this->y*zs;
T zz = this->z*zs;
m00 = 1.0 - (yy + zz);
//m11 = 1.0 - (xx + zz);
m22 = 1.0 - (xx + yy);
m10 = xy + wz;
//m01 = xy - wz;
m20 = xz - wy;
//m02 = xz + wy;
m21 = yz + wx;
//m12 = yz - wx;
phi = atan2(m21,m22);
theta = atan2(-m20,sqrt(m21*m21 + m22*m22));
psi = atan2(m10,m00);
return Vector3<T>(phi, theta, psi);
}
template<class T>
inline Vector3<T> Quaternion<T>::axis() const {
double imNorm=sqrt(x*x+y*y+z*z);
if (imNorm<std::numeric_limits<double>::min()){
return Vector3<T>(0.,0.,1.);
}
return Vector3<T>(x/imNorm, y/imNorm, z/imNorm);
}
template<class T>
inline T Quaternion<T>::angle() const{
Quaternion<T> q=normalized();
double a=2*atan2(sqrt(q.x*q.x + q.y*q.y + q.z*q.z), q.w);
return atan2(sin(a), cos(a));
}
template<class T>
inline T Quaternion<T>::norm() const{
return sqrt(w*w + x*x + y*y + z*z);
}
template<class T>
inline T Quaternion<T>::re() const{
return w;
}
template<class T>
inline Vector3<T> Quaternion<T>::im() const{
return Vector3<T>(x, y, z);
}
template<class T>
inline Quaternion<T> operator + (const Quaternion<T>& left, const Quaternion<T>& right){
return Quaternion<T>(left.w + right.w, left.x + right.x, left.y + right.y, left.z + right.z);
}
template<class T>
inline Quaternion<T> operator - (const Quaternion<T>& left, const Quaternion<T>& right){
return Quaternion<T>(left.w - right.w, left.x - right.x, left.y - right.y, left.z - right.z);
}
template<class T>
inline Quaternion<T> operator * (const Quaternion<T>& q1, const Quaternion<T>& q2){
return Quaternion<T> (q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z,
q1.y*q2.z - q2.y*q1.z + q1.w*q2.x + q2.w*q1.x,
q1.z*q2.x - q2.z*q1.x + q1.w*q2.y + q2.w*q1.y,
q1.x*q2.y - q2.x*q1.y + q1.w*q2.z + q2.w*q1.z);
}
template<class T>
inline Quaternion<T> operator * (const Quaternion<T>& q, const T s){
return Quaternion<T>(s*q.w, s*q.x, s*q.y, s*q.z);
}
template<class T>
inline Quaternion<T> operator * (const T s, const Quaternion<T>& q){
return Quaternion<T>(q.w*s, q.x*s, q.y*s, q.z*s);
}
template<class T>
std::ostream& operator << (std::ostream& os, const Quaternion<T>& q){
os << q.w << " " << q.x << " " << q.y << " " << q.z << " ";
return os;
}
template<class T>
inline T innerproduct(const Quaternion<T>& q1, const Quaternion<T>& q2){
return q1.w*q2.w + q1.x*q2.x + q1.y*q2.y + q1.z*q2.z;
}
template<class T>
inline Quaternion<T> slerp(const Quaternion<T>& from, const Quaternion<T>& to, const T lambda){
Quaternion<T> _from = from.normalized();
Quaternion<T> _to = to.normalized();
T _cos_omega = innerproduct(_from,_to);
_cos_omega = (_cos_omega>1)?1:_cos_omega;
_cos_omega = (_cos_omega<-1)?-1:_cos_omega;
T _omega = acos(_cos_omega);
assert (!isnan(_cos_omega));
if (fabs(_omega) < 1e-6)
return to;
//determine right direction of slerp:
Quaternion<T> _pq = _from - _to;
Quaternion<T> _pmq = _from + _to;
T _first = _pq.norm();
T _alternativ = _pmq.norm();
Quaternion<T> q1 = _from;
Quaternion<T> q2 = (_first < _alternativ)? (Quaternion<T>) _to: -1.*(Quaternion<T>)_to;
//now calculate intermediate quaternion.
Quaternion<T> ret = q1*(sin((1-lambda)*_omega)/(sin(_omega))) + q2*(sin(lambda*_omega)/sin(_omega));
assert (!(isnan(ret.w) || isnan(ret.x) || isnan(ret.y) || isnan(ret.z)));
return ret;
}
template <class T>
inline Transformation3<T> Transformation3<T>::identity(){
Transformation3<T> m;
m.rotationQuaternion=Quaternion<T>();
m.translationVector(0.,0.,0.);
return m;
}
template <class T>
inline Transformation3<T>::Transformation3 (const T& x, const T& y, const T& z, const T& roll, const T& pitch, const T& yaw){
rotationQuaternion=Quaternion<T>(roll,pitch,yaw);
translationVector=Vector3<T>(x,y,z);
}
template <class T>
inline Transformation3<T>::Transformation3 (const Pose3<T>& v){
rotationQuaternion=Quaternion<T>(v.roll(),v.pitch(),v.yaw());
translationVector=Vector3<T>(v.x(),v.y(),v.z());
}
template <class T>
inline Vector3<T> Transformation3<T>::translation() const {
return translationVector;
}
template <class T>
inline Quaternion<T> Transformation3<T>::rotation() const {
return rotationQuaternion;
}
template <class T>
inline Pose3<T> Transformation3<T>::toPoseType() const {
Vector3<T> t=translation();
Vector3<T> r=rotationQuaternion.toAngles();
Pose3<T> rv(t.x(), t.y(), t.z(), r.roll(), r.pitch(), r.yaw() );
return rv;
}
template <class T>
inline void Transformation3<T>::setTranslation(const Vector3<T>& t){
translationVector=t;
}
template <class T>
inline void Transformation3<T>::setRotation(const Quaternion<T>& q){
rotationQuaternion=q.normalized();
}
template <class T>
inline void Transformation3<T>::setRotation(const Vector3<T>& r){
setRotation(r.roll(),r.pitch(), r.yaw());
}
template <class T>
inline void Transformation3<T>::setRotation(const T& roll_phi, const T& pitch_theta, const T& yaw_psi){
rotationQuaternion=Quaternion<T>(roll_phi, pitch_theta, yaw_psi);
}
template <class T>
inline void Transformation3<T>::setTranslation(const T& x, const T& y, const T& z){
translationVector=Vector3<T>(x,y,z);
}
template <class T>
inline Transformation3<T> Transformation3<T>::inv() const {
Transformation3<T> rv(*this);
rv.rotationQuaternion=rotationQuaternion.inverse().normalized();
rv.translationVector=rv.rotationQuaternion.rotatePoint(translationVector*-1.);
return rv;
}
template <class T>
inline Vector3<T> operator * (const Transformation3<T>& m, const Vector3<T>& v){
return m.translationVector+m.rotationQuaternion.rotatePoint(v);
}
template <class T>
inline Transformation3<T> operator * (const Transformation3<T>& m1, const Transformation3<T>& m2){
Transformation3<T> rv;
rv.translationVector=m1.rotationQuaternion.rotatePoint(m2.translationVector)+m1.translationVector;
rv.rotationQuaternion=(m1.rotationQuaternion*m2.rotationQuaternion).normalized();
return rv;
}
} // namespace AISNavigation

368
corelib/src/optimizer/toro3d/treeoptimizer2.cpp Normal file
View File

@@ -0,0 +1,368 @@
/**********************************************************************
*
* 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>
typedef unsigned int uint;
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():
iteration(1){
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;
}
if (iteration==1)
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/optimizer/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

360
corelib/src/optimizer/toro3d/treeoptimizer3.cpp Normal file
View File

@@ -0,0 +1,360 @@
/**********************************************************************
*
* 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 treeoptimizer3.cpp
*
* \brief Defines the core optimizer class for 3D graphs which is a
* subclass of TreePoseGraph3
*
**/
#include "treeoptimizer3.hh"
#include <fstream>
#include <sstream>
#include <string>
using namespace std;
namespace AISNavigation {
//#define DEBUG(i) if (verboseLevel>i) cerr
TreeOptimizer3::TreeOptimizer3(){
restartOnDivergence=false;
sortedEdges=0;
mpl=-1;
edgeCompareMode=EVComparator<Edge*>::CompareLevel;
}
TreeOptimizer3::~TreeOptimizer3(){
}
void TreeOptimizer3::initializeTreeParameters(){
ParameterPropagator pp;
treeDepthVisit(pp,root);
}
void TreeOptimizer3::iterate(TreePoseGraph3::EdgeSet* eset, bool noPreconditioner){
TreePoseGraph3::EdgeSet* temp=sortedEdges;
if (eset){
sortedEdges=eset;
}
if (noPreconditioner)
propagateErrors(false);
else {
if (iteration==1)
computePreconditioner();
propagateErrors(true);
}
sortedEdges=temp;
onRestartBegin();
if (restartOnDivergence){
double mte, ate;
double mre, are;
error(&mre, &mte, &are, &ate);
maxTranslationalErrors.push_back(mte);
maxRotationalErrors.push_back(mre);
int interval=3;
if ((int)maxRotationalErrors.size()>=interval){
uint s=(uint)maxRotationalErrors.size();
double re0 = maxRotationalErrors[s-interval];
double re1 = maxRotationalErrors[s-1];
if ((re1-re0)>are || sqrt(re1)>0.99*M_PI){
double rg=rotGain;
if (sqrt(re1)>M_PI/4){
cerr << "RESTART!!!!! : Angular wraparound may be occourring" << endl;
cerr << " err=" << re0 << " -> " << re1 << endl;
cerr << "Restarting optimization and reducing the rotation factor" << endl;
cerr << rg << " -> ";
initializeOnTree();
initializeTreeParameters();
initializeOptimization();
error(&mre, &mte);
maxTranslationalErrors.push_back(mte);
maxRotationalErrors.push_back(mre);
rg*=0.1;
rotGain=rg;
cerr << rotGain << endl;
}
else {
cerr << "decreasing angular gain" << rotGain*0.1 << endl;
rotGain*=0.1;
}
}
}
}
onRestartDone();
}
void TreeOptimizer3::recomputeTransformations(Vertex*v, Vertex* top){
if (v==top)
return;
recomputeTransformations(v->parent, top);
v->transformation=v->parent->transformation*v->parameters;
}
void TreeOptimizer3::recomputeParameters(Vertex*v, Vertex* top){
while (v!=top){
v->parameters=v->parent->transformation.inv()*v->transformation;
v=v->parent;
}
}
TreeOptimizer3::Transformation TreeOptimizer3::getPose(Vertex*v, Vertex* top){
Transformation t(0.,0.,0.,0.,0.,0.);
if (v==top)
return v->transformation;
while (v!=top){
t=v->parameters*t;
v=v->parent;
}
return top->transformation*t;
}
TreeOptimizer3::Rotation TreeOptimizer3::getRotation(Vertex*v, Vertex* top){
Rotation r(0.,0.,0.);
if (v==top)
return v->transformation.rotation();
while (v!=top){
r=v->parameters.rotation()*r;
v=v->parent;
}
return top->transformation.rotation()*r;
}
double TreeOptimizer3::error(const Edge* e) const{
const Vertex* v1=e->v1;
const Vertex* v2=e->v2;
Transformation et=e->transformation;
Transformation t1=v1->transformation;
Transformation t2=v2->transformation;
Transformation t12=(t1*et)*t2.inv();
Pose p12=t12.toPoseType();
Pose ps=e->informationMatrix*p12;
double err=p12*ps;
//DEBUG(100) << "e(" << v1->id << "," << v2->id << ")" << err << endl;
return err;
}
double TreeOptimizer3::traslationalError(const Edge* e) const{
const Vertex* v1=e->v1;
const Vertex* v2=e->v2;
Transformation et=e->transformation;
Transformation t1=v1->transformation;
Transformation t2=v2->transformation;
Translation t12=(t2.inv()*(t1*et)).translation();
return t12*t12;;
}
double TreeOptimizer3::rotationalError(const Edge* e) const{
const Vertex* v1=e->v1;
const Vertex* v2=e->v2;
Rotation er=e->transformation.rotation();
Rotation r1=v1->transformation.rotation();
Rotation r2=v2->transformation.rotation();
Rotation r12=r2.inverse()*(r1*er);
double r=r12.angle();
return r*r;
}
double TreeOptimizer3::loopError(const Edge* e) const{
double err=0;
const Vertex* v=e->v1;
while (v!=e->top){
err+=error(v->parentEdge);
v=v->parent;
}
v=e->v2;
while (v==e->top){
err+=error(v->parentEdge);
v=v->parent;
}
if (e->v2->parentEdge!=e && e->v1->parentEdge!=e)
err+=error(e);
return err;
}
double TreeOptimizer3::loopRotationalError(const Edge* e) const{
double err=0;
const Vertex* v=e->v1;
while (v!=e->top){
err+=rotationalError(v->parentEdge);
v=v->parent;
}
v=e->v2;
while (v!=e->top){
err+=rotationalError(v->parentEdge);
v=v->parent;
}
if (e->v2->parentEdge!=e && e->v1->parentEdge!=e)
err+=rotationalError(e);
return err;
}
double TreeOptimizer3::error(double* mre, double* mte, double* are, double* ate, TreePoseGraph3::EdgeSet* eset) const{
double globalRotError=0.;
double maxRotError=0;
double globalTrasError=0.;
double maxTrasError=0;
int c=0;
if (! eset){
for (TreePoseGraph3::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
double re=rotationalError(it->second);
globalRotError+=re;
maxRotError=maxRotError>re?maxRotError:re;
double te=traslationalError(it->second);
globalTrasError+=te;
maxTrasError=maxTrasError>te?maxTrasError:te;
c++;
}
} else {
for (TreePoseGraph3::EdgeSet::const_iterator it=eset->begin(); it!=eset->end(); it++){
const TreePoseGraph3::Edge* edge=*it;
double re=rotationalError(edge);
globalRotError+=re;
maxRotError=maxRotError>re?maxRotError:re;
double te=traslationalError(edge);
globalTrasError+=te;
maxTrasError=maxTrasError>te?maxTrasError:te;
c++;
}
}
if (mte)
*mte=maxTrasError;
if (mre)
*mre=maxRotError;
if (ate)
*ate=globalTrasError/c;
if (are)
*are=globalRotError/c;
return globalRotError+globalTrasError;
}
void TreeOptimizer3::initializeOptimization(EdgeCompareMode mode){
edgeCompareMode=mode;
// 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();
mpl=maxPathLength();
rotGain=1.;
trasGain=1.;
}
void TreeOptimizer3::initializeOnlineIterations(){
int sz=maxIndex()+1;
//DEBUG(1) << "Size= " << sz << endl;
M.resize(sz);
//DEBUG(1) << "allocating M(" << sz << ")" << endl;
iteration=1;
maxRotationalErrors.clear();
maxTranslationalErrors.clear();
rotGain=1.;
trasGain=1.;
}
void TreeOptimizer3::initializeOnlineOptimization(EdgeCompareMode mode){
edgeCompareMode=mode;
// compute the size of the preconditioning matrix
clear();
Vertex* v0=addVertex(0,Pose(0,0,0,0,0,0));
root=v0;
v0->parameters=Transformation(v0->pose);
v0->parentEdge=0;
v0->parent=0;
v0->level=0;
v0->transformation=Transformation(TreePoseGraph3::Pose(0,0,0,0,0,0));
}
void TreeOptimizer3::onStepStart(Edge* e){
//DEBUG(5) << "entering edge" << e << endl;
}
void TreeOptimizer3::onStepFinished(Edge* e){
//DEBUG(5) << "exiting edge" << e << endl;
}
void TreeOptimizer3::onIterationStart(int iteration){
//DEBUG(5) << "entering iteration " << iteration << endl;
}
void TreeOptimizer3::onIterationFinished(int iteration){
//DEBUG(5) << "exiting iteration " << iteration << endl;
}
void TreeOptimizer3::onRestartBegin(){}
void TreeOptimizer3::onRestartDone(){}
bool TreeOptimizer3::isDone(){
return false;
}
}; //namespace AISNavigation

181
corelib/src/optimizer/toro3d/treeoptimizer3.hh Normal file
View File

@@ -0,0 +1,181 @@
/**********************************************************************
*
* 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 treeoptimizer3.hh
*
* \brief Defines the core optimizer class for 3D graphs which is a
* subclass of TreePoseGraph3
*
**/
#ifndef _TREEOPTIMIZER3_HH_
#define _TREEOPTIMIZER3_HH_
#include "posegraph3.hh"
namespace AISNavigation {
/** \brief Class that contains the core optimization algorithm **/
struct TreeOptimizer3: public TreePoseGraph3{
typedef std::vector<Pose> PoseVector;
/** Constructor **/
TreeOptimizer3();
/** Destructor **/
virtual ~TreeOptimizer3();
/** Initialization function **/
void initializeTreeParameters();
/** Initialization function **/
void initializeOptimization(EdgeCompareMode mode=EVComparator<Edge*>::CompareLevel);
void initializeOnlineOptimization(EdgeCompareMode mode=EVComparator<Edge*>::CompareLevel);
void initializeOnlineIterations();
/** Performs one iteration of the algorithm **/
void iterate(TreePoseGraph3::EdgeSet* eset=0, bool noPreconditioner=false);
/** Conmputes the gloabl error of the network **/
double error(double* mre=0, double* mte=0, double* are=0, double* ate=0, TreePoseGraph3::EdgeSet* eset=0) const;
/** Conmputes the gloabl error of the network **/
double angularError() const;
/** Conmputes the gloabl error of the network **/
double translationalError() const;
bool restartOnDivergence;
inline double getRotGain() const {return rotGain;}
/** Iteration counter **/
int iteration;
double rpFraction;
protected:
/** Recomputes only the pose of the node v wrt. to an arbitraty
parent (top) of v in the tree **/
Transformation getPose(Vertex*v, Vertex* top);
/** Recomputes only the pose of the node v wrt. to an arbitraty
parent (top) of v in the tree **/
Rotation getRotation(Vertex*v, Vertex* top);
void recomputeTransformations(Vertex*v, Vertex* top);
void recomputeParameters(Vertex*v, Vertex* top);
void computePreconditioner();
void propagateErrors(bool usePreconditioner=false);
/** Computes the error of the constraint/edge e **/
double error(const Edge* e) const;
/** Computes the error of the constraint/edge e **/
double loopError(const Edge* e) const;
/** Computes the rotational error of the constraint/edge e **/
double loopRotationalError(const Edge* e) const;
/** Conmputes the error of the constraint/edge e **/
double translationalError(const Edge* e) const;
/** Conmputes the error of the constraint/edge e **/
double rotationalError(const Edge* e) const;
double traslationalError(const Edge* e) const;
/** Used to compute the learning rate lambda **/
double gamma[2];
/** The simplified version of the preconditioning matrix **/
struct PM_t{
double v [2];
inline double& operator[](int i){return v[i];}
};
typedef std::vector< PM_t > PMVector;
PMVector M;
/**cached maximum path length*/
int mpl;
/**history of rhe maximum rotational errors*, used when adaptiveRestart is enabled */
std::vector<double> maxRotationalErrors;
/**history of rhe maximum rotational errors*, used when adaptiveRestart is enabled */
std::vector<double> maxTranslationalErrors;
double rotGain, trasGain;
/**callback invoked before starting the optimization of an individual constraint,
@param e: the constraint being optimized*/
virtual void onStepStart(Edge* e);
/**callback invoked after finishing the optimization of an individual constraint,
@param e: the constraint optimized*/
virtual void onStepFinished(Edge* e);
/**callback invoked before starting a full iteration,
@param i: the current iteration number*/
virtual void onIterationStart(int i);
/**callback invoked after finishing a full iteration,
@param i: the current iteration number*/
virtual void onIterationFinished(int iteration);
/**callback invoked before a restart of the optimizer
when the angular wraparound is detected*/
virtual void onRestartBegin();
/**callback invoked after a restart of the optimizer*/
virtual void onRestartDone();
/**callback for determining a termination condition,
it can be used by an external thread for stopping the optimizer while performing an iteration.
@returns true when the optimizer has to stop.*/
virtual bool isDone();
};
}; //namespace AISNavigation
#endif

342
corelib/src/optimizer/toro3d/treeoptimizer3_iteration.cpp Normal file
View File

@@ -0,0 +1,342 @@
/**********************************************************************
*
* 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.
**********************************************************************/
#include "treeoptimizer3.hh"
#include <fstream>
#include <string>
using namespace std;
namespace AISNavigation {
//#define DEBUG(i) if (verboseLevel>i) cerr
//helper functions. Should I explain :-)?
inline double max3( const double& a, const double& b, const double& c){
double m=a>b?a:b;
return m>c?m:c;
}
inline double min3( const double& a, const double& b, const double& c){
double m=a<b?a:b;
return m<c?m:c;
}
struct NodeInfo{
TreeOptimizer3::Vertex* n;
double translationalWeight;
double rotationalWeight;
int direction;
TreeOptimizer3::Transformation transformation;
TreeOptimizer3::Transformation parameters;
NodeInfo(TreeOptimizer3::Vertex* v=0, double tw=0, double rw=0, int dir=0,
TreeOptimizer3::Transformation t=TreeOptimizer3::Transformation(0,0,0,0,0,0),
TreeOptimizer3::Parameters p=TreeOptimizer3::Transformation(0,0,0,0,0,0)){
n=v;
translationalWeight=tw;
rotationalWeight=rw;
direction=dir;
transformation=t;
parameters=p;
}
};
typedef std::vector<NodeInfo> NodeInfoVector;
/********************************** Preconditioned and unpreconditioned error distribution ************************************/
void TreeOptimizer3::computePreconditioner(){
for (uint i=0; i<M.size(); i++){
M[i][0]=0;
M[i][1]=0;
}
gamma[0] = gamma[1] = numeric_limits<double>::max();
int edgeCount=0;
for (EdgeSet::iterator it=sortedEdges->begin(); it!=sortedEdges->end(); it++){
edgeCount++;
//if (! (edgeCount%1000))
// DEBUG(1) << "m";
Edge* e=*it;
//Transformation t=e->transformation;
InformationMatrix W=e->informationMatrix;
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;
double rW=min3(W[0][0], W[1][1], W[2][2]);
double tW=min3(W[3][3], W[4][4], W[5][5]);
M[i][0]+=rW;
M[i][1]+=tW;
gamma[0]=gamma[0]<rW?gamma[0]:rW;
gamma[1]=gamma[1]<tW?gamma[1]:tW;
n=n->parent;
}
}
}
if (verboseLevel>1){
for (uint i=0; i<M.size(); i++){
cerr << "M[" << i << "]=" << M[i][0] << " " << M[i][1] << endl;
}
}
}
void TreeOptimizer3::propagateErrors(bool usePreconditioner){
iteration++;
int edgeCount=0;
// this is the workspace for computing the paths without
// bothering too much the memory allocation
static NodeInfoVector path;
path.resize(edges.size()+1);
static Rotation zero(0.,0.,0.);
onIterationStart(iteration);
for (EdgeSet::iterator it=sortedEdges->begin(); it!=sortedEdges->end(); it++){
edgeCount++;
//if (! (edgeCount%1000))
// DEBUG(1) << "c";
if (isDone())
return;
Edge* e=*it;
Vertex* top=e->top;
Vertex* v1=e->v1;
Vertex* v2=e->v2;
int l=e->length;
onStepStart(e);
recomputeTransformations(v1,top);
recomputeTransformations(v2,top);
//DEBUG(2) << "Edge: " << v1->id << " " << v2->id << ", top=" << top->id << ", length="<< l <<endl;
//BEGIN: Path and weight computation
int pc=0;
Vertex* aux=v1;
double totTW=0, totRW=0;
while(aux!=top){
int index=aux->id;
double tw=1./(double)l, rw=1./(double)l;
if (usePreconditioner){
tw=1./M[index][0];
rw=1./M[index][1];
}
totTW+=tw;
totRW+=rw;
path[pc++]=NodeInfo(aux,tw,rw,-1,aux->transformation, aux->parameters);
aux=aux->parent;
}
int topIndex=pc;
path[pc++]=NodeInfo(top,0.,0.,0, top->transformation, top->parameters);
pc=l;
aux=v2;
while(aux!=top){
int index=aux->id;
double tw=1./l, rw=1./l;
if (usePreconditioner){
tw=1./M[index][0];
rw=1./M[index][1];
}
totTW+=tw;
totRW+=rw;
path[pc--]=NodeInfo(aux,tw,rw,1,aux->transformation, aux->parameters);
aux=aux->parent;
}
//store the transformations relative to the top node
//Transformation topTransformation=top->transformation;
//Transformation topParameters=top->parameters;
//END: Path and weight computation
//BEGIN: Rotational Error
Rotation r1=getRotation(v1, top);
Rotation r2=getRotation(v2, top);
Rotation re=e->transformation.rotation();
Rotation rR=r2.inverse()*(r1*re);
double rotationFactor=(usePreconditioner)?
sqrt(double(l))* min3(e->informationMatrix[0][0],
e->informationMatrix[1][1],
e->informationMatrix[2][2])/
( gamma[0]* (double)iteration ):
sqrt(double(l))*rotGain/(double)iteration;
// double rotationFactor=(usePreconditioner)?
// sqrt(double(l))*rotGain/
// ( gamma[0]* (double)iteration * min3(e->informationMatrix[0][0],
// e->informationMatrix[1][1],
// e->informationMatrix[2][2])):
// sqrt(double(l))*rotGain/(double)iteration;
if (rotationFactor>1)
rotationFactor=1;
Rotation totalRotation = path[l].transformation.rotation() * rR * path[l].transformation.rotation().inverse();
Translation axis = totalRotation.axis();
double angle=totalRotation.angle();
double cw=0;
for (int i= 1; i<=topIndex; i++){
cw+=path[i-1].rotationalWeight/totRW;
Rotation R=path[i].transformation.rotation();
Rotation B(axis, angle*cw*rotationFactor);
R= B*R;
path[i].transformation.setRotation(R);
}
for (int i= topIndex+1; i<=l; i++){
cw+=path[i].rotationalWeight/totRW;
Rotation R=path[i].transformation.rotation();
Rotation B(axis, angle*cw*rotationFactor);
R= B*R;
path[i].transformation.setRotation(R);
}
//recompute the parameters based on the transformation
for (int i=0; i<topIndex; i++){
Vertex* n=path[i].n;
n->parameters.setRotation(path[i+1].transformation.rotation().inverse()*path[i].transformation.rotation());
}
for (int i= topIndex+1; i<=l; i++){
Vertex* n=path[i].n;
n->parameters.setRotation(path[i-1].transformation.rotation().inverse()*path[i].transformation.rotation());
}
//END: Rotational Error
//now spread the parameters
recomputeTransformations(v1,top);
recomputeTransformations(v2,top);
//BEGIN: Translational Error
//Translation topTranslation=top->transformation.translation();
Transformation tr12=v1->transformation*e->transformation;
Translation tR=tr12.translation()-v2->transformation.translation();
// double translationFactor=(usePreconditioner)?
// trasGain*l/( gamma[1]* (double)iteration * min3(e->informationMatrix[3][3],
// e->informationMatrix[4][4],
// e->informationMatrix[5][5])):
// trasGain*l/(double)iteration;
double translationFactor=(usePreconditioner)?
trasGain*l*min3(e->informationMatrix[3][3],
e->informationMatrix[4][4],
e->informationMatrix[5][5])/( gamma[1]* (double)iteration):
trasGain*l/(double)iteration;
if (translationFactor>1)
translationFactor=1;
Translation dt=tR*translationFactor;
//left wing
double lcum=0;
for (int i=topIndex-1; i>=0; i--){
Vertex* n=path[i].n;
lcum-=(usePreconditioner) ? path[i].translationalWeight/totTW : 1./(double)l;
double fraction=lcum;
Translation offset= dt*fraction;
Translation T=n->transformation.translation()+offset;
n->transformation.setTranslation(T);
}
//right wing
double rcum=0;
for (int i=topIndex+1; i<=l; i++){
Vertex* n=path[i].n;
rcum+=(usePreconditioner) ? path[i].translationalWeight/totTW : 1./(double)l;
double fraction=rcum;
Translation offset= dt*fraction;
Translation T=n->transformation.translation()+offset;
n->transformation.setTranslation(T);
}
assert(fabs(lcum+rcum)-1<1e-6);
recomputeParameters(v1, top);
recomputeParameters(v2, top);
//END: Translational Error
onStepFinished(e);
if (verboseLevel>2){
Rotation newRotResidual=v2->transformation.rotation().inverse()*(v1->transformation.rotation()*re);
Translation newRotResidualAxis=newRotResidual.axis();
double newRotResidualAngle=newRotResidual.angle();
Translation rotResidualAxis=rR.axis();
double rotResidualAngle=rR.angle();
Translation newTransResidual=(v1->transformation*e->transformation).translation()-v2->transformation.translation();
cerr << "RotationalFraction: " << rotationFactor << endl;
cerr << "Rotational residual: "
<< " axis " << rotResidualAxis.x() << "\t" << rotResidualAxis.y() << "\t" << rotResidualAxis.z() << " --> "
<< " -> " << newRotResidualAxis.x() << "\t" << newRotResidualAxis.y() << "\t" << newRotResidualAxis.z() << endl;
cerr << " angle " << rotResidualAngle << "\t" << newRotResidualAngle << endl;
cerr << "Translational Fraction: " << translationFactor << endl;
cerr << "Translational Residual" << endl;
cerr << " " << tR.x() << "\t" << tR.y() << "\t" << tR.z() << endl;
cerr << " " << newTransResidual.x() << "\t" << newTransResidual.y() << "\t" << newTransResidual.z() << endl;
}
if (verboseLevel>101){
char filename [1000];
sprintf(filename, "po-%02d-%03d-%03d-.dat", iteration, v1->id, v2->id);
recomputeAllTransformations();
saveGnuplot(filename);
}
}
onIterationFinished(iteration);
}
};//namespace AISNavigation