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

0.11.2: First Google Tango release

Browse Source
This commit is contained in:
matlabbe
2016-02-15 19:35:24 -05:00
parent 8dc01c491b
commit c6a46a7238
415 changed files with 73300 additions and 3193 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
corelib/src/toro3d/posegraph2.cpp
View File

@@ -54,8 +54,8 @@ typedef unsigned int uint;
#define LINESIZE 81920
#define DEBUG(i) \
if (verboseLevel>i) cerr
//#define DEBUG(i) \
// if (verboseLevel>i) cerr
bool TreePoseGraph2::load(const char* filename, bool overrideCovariances){
@@ -75,8 +75,8 @@ bool TreePoseGraph2::load(const char* filename, bool overrideCovariances){
int id;
Pose p;
ls >> id >> p.x() >> p.y() >> p.theta();
if (addVertex(id,p))
DEBUG(2) << "V " << id << endl;
addVertex(id,p);
//DEBUG(2) << "V " << id << endl;
}
@@ -98,8 +98,8 @@ bool TreePoseGraph2::load(const char* filename, bool overrideCovariances){
TreePoseGraph2::Vertex* v1=vertex(id1);
TreePoseGraph2::Vertex* v2=vertex(id2);
Transformation t(p);
if (addEdge(v1, v2,t ,m))
DEBUG(2) << "E " << id1 << " " << id2 << endl;
addEdge(v1, v2,t ,m);
//DEBUG(2) << "E " << id1 << " " << id2 << endl;
}
}
return true;
@@ -411,11 +411,11 @@ void TreePoseGraph2::collapseEdge(Edge* e){
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;
//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;
//DEBUG(2) << "p1x_final= " << pFinal.x() << " " << pFinal.y() << " " << pFinal.theta() << endl;
e1x->transformation=Transformation(pFinal);
e1x->informationMatrix=IM;

4
corelib/src/toro3d/posegraph3.cpp
View File

@@ -51,8 +51,8 @@ namespace AISNavigation {
#define LINESIZE 81920
#define DEBUG(i) \
if (verboseLevel>i) cerr
//#define DEBUG(i) \
// if (verboseLevel>i) cerr
bool TreePoseGraph3::load(const char* filename, bool overrideCovariances, bool twoDimensions){

54
corelib/src/toro3d/treeoptimizer2.cpp
View File

@@ -51,8 +51,8 @@ using namespace std;
namespace AISNavigation {
#define DEBUG(i) \
if (verboseLevel>i) cerr
//#define DEBUG(i) \
// if (verboseLevel>i) cerr
/** \brief A class (struct) to compute the parameterization of the vertex v **/
struct ParameterPropagator{
@@ -83,9 +83,9 @@ void TreeOptimizer2::initializeTreeParameters(){
void TreeOptimizer2::initializeOptimization(){
// compute the size of the preconditioning matrix
int sz=maxIndex()+1;
DEBUG(1) << "Size= " << sz << endl;
//DEBUG(1) << "Size= " << sz << endl;
M.resize(sz);
DEBUG(1) << "allocating M(" << sz << ")" << endl;
//DEBUG(1) << "allocating M(" << sz << ")" << endl;
iteration=1;
// sorting edges
@@ -99,9 +99,9 @@ void TreeOptimizer2::initializeOptimization(){
void TreeOptimizer2::initializeOnlineOptimization(){
// compute the size of the preconditioning matrix
int sz=maxIndex()+1;
DEBUG(1) << "Size= " << sz << endl;
//DEBUG(1) << "Size= " << sz << endl;
M.resize(sz);
DEBUG(1) << "allocating M(" << sz << ")" << endl;
//DEBUG(1) << "allocating M(" << sz << ")" << endl;
iteration=1;
}
@@ -114,8 +114,8 @@ void TreeOptimizer2::computePreconditioner(){
int edgeCount=0;
for (EdgeSet::iterator it=sortedEdges->begin(); it!=sortedEdges->end(); it++){
edgeCount++;
if (! (edgeCount%10000))
DEBUG(1) << "m";
//if (! (edgeCount%10000))
// DEBUG(1) << "m";
Edge* e=*it;
Transformation t=e->transformation;
@@ -166,7 +166,7 @@ void TreeOptimizer2::propagateErrors(){
for (EdgeSet::iterator it=sortedEdges->begin(); it!=sortedEdges->end(); it++){
edgeCount++;
if (! (edgeCount%10000)) DEBUG(1) << "c";
//if (! (edgeCount%10000)) DEBUG(1) << "c";
Edge* e=*it;
Vertex* top=e->top;
@@ -176,13 +176,13 @@ void TreeOptimizer2::propagateErrors(){
Vertex* v2=e->v2;
double l=e->length;
DEBUG(2) << "Edge: " << v1->id << " " << v2->id << ", top=" << top->id << ", length="<< l <<endl;
//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;
//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);
@@ -191,13 +191,13 @@ void TreeOptimizer2::propagateErrors(){
Transformation t12=t1*et;
Pose p12=t12.toPoseType();
DEBUG(2) << " pt2=" << p12.x() << " " << p12.y() << " " << p12.theta() << endl;
//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;
//DEBUG(2) << " e=" << r.x() << " " << r.y() << " " << r.theta() << endl;
InformationMatrix S=e->informationMatrix;
InformationMatrix R;
@@ -216,7 +216,7 @@ void TreeOptimizer2::propagateErrors(){
InformationMatrix W=R*S*R.transpose();
Pose d=W*r*2.;
DEBUG(2) << " d=" << d.x() << " " << d.y() << " " << d.theta() << endl;
//DEBUG(2) << " d=" << d.x() << " " << d.y() << " " << d.theta() << endl;
assert(l>0);
@@ -239,8 +239,8 @@ void TreeOptimizer2::propagateErrors(){
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;
//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;
@@ -253,13 +253,13 @@ void TreeOptimizer2::propagateErrors(){
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() << " -> ";
//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;
//DEBUG(2) << n->parameters.x() << " " << n->parameters.y() << " " << n->parameters.theta()<< endl;
n=n->parent;
}
}
@@ -269,9 +269,9 @@ void TreeOptimizer2::propagateErrors(){
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;
//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;
}
}
@@ -320,8 +320,8 @@ double TreeOptimizer2::error(const Edge* e) const{
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;
//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);
@@ -330,13 +330,13 @@ double TreeOptimizer2::error(const Edge* e) const{
Transformation t12=t1*et;
Pose p12=t12.toPoseType();
DEBUG(2) << " pt2=" << p12.x() << " " << p12.y() << " " << p12.theta() << endl;
//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;
//DEBUG(2) << " e=" << r.x() << " " << r.y() << " " << r.theta() << endl;
InformationMatrix S=e->informationMatrix;
InformationMatrix R;

22
corelib/src/toro3d/treeoptimizer3.cpp
View File

@@ -50,8 +50,8 @@ using namespace std;
namespace AISNavigation {
#define DEBUG(i) \
if (verboseLevel>i) cerr
//#define DEBUG(i) \
// if (verboseLevel>i) cerr
TreeOptimizer3::TreeOptimizer3(){
@@ -180,7 +180,7 @@ double TreeOptimizer3::error(const Edge* e) const{
Pose ps=e->informationMatrix*p12;
double err=p12*ps;
DEBUG(100) << "e(" << v1->id << "," << v2->id << ")" << err << endl;
//DEBUG(100) << "e(" << v1->id << "," << v2->id << ")" << err << endl;
return err;
}
@@ -294,10 +294,10 @@ void TreeOptimizer3::initializeOptimization(EdgeCompareMode mode){
edgeCompareMode=mode;
// compute the size of the preconditioning matrix
int sz=maxIndex()+1;
DEBUG(1) << "Size= " << sz << endl;
//DEBUG(1) << "Size= " << sz << endl;
M.resize(sz);
DEBUG(1) << "allocating M(" << sz << ")" << endl;
//DEBUG(1) << "allocating M(" << sz << ")" << endl;
iteration=1;
// sorting edges
@@ -313,9 +313,9 @@ void TreeOptimizer3::initializeOptimization(EdgeCompareMode mode){
void TreeOptimizer3::initializeOnlineIterations(){
int sz=maxIndex()+1;
DEBUG(1) << "Size= " << sz << endl;
//DEBUG(1) << "Size= " << sz << endl;
M.resize(sz);
DEBUG(1) << "allocating M(" << sz << ")" << endl;
//DEBUG(1) << "allocating M(" << sz << ")" << endl;
iteration=1;
maxRotationalErrors.clear();
maxTranslationalErrors.clear();
@@ -337,18 +337,18 @@ void TreeOptimizer3::initializeOnlineOptimization(EdgeCompareMode mode){
}
void TreeOptimizer3::onStepStart(Edge* e){
DEBUG(5) << "entering edge" << e << endl;
//DEBUG(5) << "entering edge" << e << endl;
}
void TreeOptimizer3::onStepFinished(Edge* e){
DEBUG(5) << "exiting edge" << e << endl;
//DEBUG(5) << "exiting edge" << e << endl;
}
void TreeOptimizer3::onIterationStart(int iteration){
DEBUG(5) << "entering iteration " << iteration << endl;
//DEBUG(5) << "entering iteration " << iteration << endl;
}
void TreeOptimizer3::onIterationFinished(int iteration){
DEBUG(5) << "exiting iteration " << iteration << endl;
//DEBUG(5) << "exiting iteration " << iteration << endl;
}
void TreeOptimizer3::onRestartBegin(){}

14
corelib/src/toro3d/treeoptimizer3_iteration.cpp
View File

@@ -42,8 +42,8 @@ using namespace std;
namespace AISNavigation {
#define DEBUG(i) \
if (verboseLevel>i) cerr
//#define DEBUG(i) \
// if (verboseLevel>i) cerr
//helper functions. Should I explain :-)?
inline double max3( const double& a, const double& b, const double& c){
@@ -94,8 +94,8 @@ void TreeOptimizer3::computePreconditioner(){
int edgeCount=0;
for (EdgeSet::iterator it=sortedEdges->begin(); it!=sortedEdges->end(); it++){
edgeCount++;
if (! (edgeCount%1000))
DEBUG(1) << "m";
//if (! (edgeCount%1000))
// DEBUG(1) << "m";
Edge* e=*it;
//Transformation t=e->transformation;
@@ -138,8 +138,8 @@ void TreeOptimizer3::propagateErrors(bool usePreconditioner){
onIterationStart(iteration);
for (EdgeSet::iterator it=sortedEdges->begin(); it!=sortedEdges->end(); it++){
edgeCount++;
if (! (edgeCount%1000))
DEBUG(1) << "c";
//if (! (edgeCount%1000))
// DEBUG(1) << "c";
if (isDone())
return;
@@ -153,7 +153,7 @@ void TreeOptimizer3::propagateErrors(bool usePreconditioner){
recomputeTransformations(v1,top);
recomputeTransformations(v2,top);
DEBUG(2) << "Edge: " << v1->id << " " << v2->id << ", top=" << top->id << ", length="<< l <<endl;
//DEBUG(2) << "Edge: " << v1->id << " " << v2->id << ", top=" << top->id << ", length="<< l <<endl;
//BEGIN: Path and weight computation
int pc=0;